From 45d1285acb6492e450f166caaf8f88c91c5ba069 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 16:19:10 +0200 Subject: [PATCH 01/51] chore(pylint): add .pylintrc configuration file --- .pylintrc | 656 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 656 insertions(+) create mode 100644 .pylintrc diff --git a/.pylintrc b/.pylintrc new file mode 100644 index 000000000..454f365b0 --- /dev/null +++ b/.pylintrc @@ -0,0 +1,656 @@ +[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=17 + +# 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=63 + +# 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=148 + +# 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, + bad-inline-option, + locally-disabled, + file-ignored, + suppressed-message, + useless-suppression, + deprecated-pragma, + use-symbolic-message-instead, + use-implicit-booleaness-not-comparison-to-string, + use-implicit-booleaness-not-comparison-to-zero, + invalid-name, # We need to fix this, but it takes too fucking long + import-error, # E0401: Unable to import + broad-exception-caught, # Someone who knows the code needs to fix this. + too-few-public-methods, # Someone needs to make this class into just two methods + duplicate-code, # Can be ignored, but someone might need to improve the code + dangerous-default-value, # Someone else who knows what they are doing needs to fix this + inconsistent-return-statements, # Need to ask someone who knows how to fix this + no-name-in-module, + unknown-option-value, + +# 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 From 7727642ba229d50d823bc451caf2a36d6b4fb8c2 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 16:19:29 +0200 Subject: [PATCH 02/51] refactor: fix pylint warning W1510 --- mission/LCD/sources/temperature_sensor_lib.py | 1 + .../internal_status_auv/temperature_sensor_lib.py | 1 + 2 files changed, 2 insertions(+) diff --git a/mission/LCD/sources/temperature_sensor_lib.py b/mission/LCD/sources/temperature_sensor_lib.py index 807a672d1..3925fed4a 100755 --- a/mission/LCD/sources/temperature_sensor_lib.py +++ b/mission/LCD/sources/temperature_sensor_lib.py @@ -22,6 +22,7 @@ def get_temperature(self): ["cat", self.temperature_sensor_file_location], capture_output=True, text=True, + check=False, ) # Decode and strip to get rid of possible newline characters 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 807a672d1..3925fed4a 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 @@ -22,6 +22,7 @@ def get_temperature(self): ["cat", self.temperature_sensor_file_location], capture_output=True, text=True, + check=False, ) # Decode and strip to get rid of possible newline characters From b17fd24cef3825a5c4ba6bbfaee5d17db627e7d9 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 17:14:11 +0200 Subject: [PATCH 03/51] refactor: fix pylint warning C0116 --- .../acoustics_data_record_lib.py | 18 ++++ .../acoustics_data_record_node.py | 80 ++++++++++++++++- .../launch/acoustics_data_record.launch.py | 11 +++ .../utilities/display_acoustics_data_live.py | 70 +++++++++++++++ .../acoustics_interface_node.py | 16 ++++ .../launch/acoustics_interface_launch.py | 11 +++ auv_setup/launch/orca.launch.py | 12 +++ auv_setup/launch/topside.launch.py | 11 +++ mission/LCD/sources/IP_lib.py | 6 ++ mission/LCD/sources/LCD.py | 10 +++ mission/LCD/sources/LCD_lib.py | 30 +++++++ mission/LCD/sources/power_sense_module_lib.py | 21 +++++ mission/LCD/sources/pressure_sensor_lib.py | 6 ++ mission/LCD/sources/temperature_sensor_lib.py | 9 ++ .../blackbox/blackbox/blackbox_log_data.py | 49 ++++++++++- mission/blackbox/blackbox/blackbox_node.py | 86 +++++++++++++++++++ mission/blackbox/launch/blackbox.launch.py | 10 +++ .../power_sense_module_lib.py | 24 ++++++ .../power_sense_module_node.py | 25 +++++- .../pressure_sensor_lib.py | 12 +++ .../pressure_sensor_node.py | 25 +++++- .../temperature_sensor_lib.py | 11 +++ .../temperature_sensor_node.py | 25 +++++- .../launch/internal_status_auv.launch.py | 16 ++++ .../joystick_interface_auv.py | 7 ++ .../launch/joystick_interface_auv.launch.py | 11 +++ .../tests/test_joystick_interface_auv.py | 75 ++++++++++++++++ .../launch/thrust_allocator_auv.launch.py | 13 +++ .../launch/thruster_interface_auv.launch.py | 12 +++ .../thruster_interface_auv_node.py | 11 +++ 30 files changed, 716 insertions(+), 7 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 c1c7b88cb..315c20239 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 @@ -47,6 +47,24 @@ def log_data_to_csv_file( tdoa=[0.0], position=[0.0], ): + """ + 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 miliseconds 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 3a03063f2..71d5d2b64 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,39 +133,104 @@ def __init__(self): "/acoustics/position [Float32MultiArray] \n" ) - # Callback methods for diffrenet topics + # Callback methods for different topics def hydrophone1_callback(self, msg): + """ + Callback method for hydrophone1 topic. + + Args: + msg (Int32MultiArray): Message containing hydrophone1 data. + """ self.hydropone1Data = msg.data def hydrophone2_callback(self, msg): + """ + Callback method for hydrophone2 topic. + + Args: + msg (Int32MultiArray): Message containing hydrophone2 data. + """ self.hydropone2Data = msg.data def hydrophone3_callback(self, msg): + """ + Callback method for hydrophone3 topic. + + Args: + msg (Int32MultiArray): Message containing hydrophone3 data. + """ self.hydropone3Data = msg.data def hydrophone4_callback(self, msg): + """ + Callback method for hydrophone4 topic. + + Args: + msg (Int32MultiArray): Message containing hydrophone4 data. + """ self.hydropone4Data = msg.data def hydrophone5_callback(self, msg): + """ + Callback method for hydrophone5 topic. + + Args: + msg (Int32MultiArray): Message containing hydrophone5 data. + """ self.hydropone5Data = msg.data def filter_response_callback(self, msg): + """ + Callback method for filter_response topic. + + Args: + msg (Int32MultiArray): Message containing filter response data. + """ self.filterResponseData = msg.data def fft_callback(self, msg): + """ + Callback method for fft topic. + + Args: + msg (Int32MultiArray): Message containing FFT data. + """ self.FFTData = msg.data def peaks_callback(self, msg): + """ + Callback method for peaks topic. + + Args: + msg (Int32MultiArray): Message containing peaks data. + """ self.peaksData = msg.data def tdoa_callback(self, msg): + """ + Callback method for time_difference_of_arrival topic. + + Args: + msg (Float32MultiArray): Message containing TDOA data. + """ self.TDOAData = msg.data def position_callback(self, msg): + """ + Callback method for position topic. + + Args: + msg (Float32MultiArray): Message containing position data. + """ self.positionData = msg.data # The logger that logs all the data def logger(self): + """ + Logs all the data to a CSV file using the AcousticsDataRecordLib. + + This method is called periodically based on the data logging rate. + """ self.acoustics_data_record.log_data_to_csv_file( hydrophone1=self.hydropone1Data, hydrophone2=self.hydropone2Data, @@ -181,6 +246,19 @@ def logger(self): def main(): + """ + Main function to initialize and run the ROS2 node for acoustics data recording. + + This function performs the following steps: + 1. Initializes the ROS2 communication. + 2. Creates an instance of the AcousticsDataRecordNode. + 3. Spins the node to keep it running until an external shutdown signal is received. + 4. Destroys the node explicitly once ROS2 stops running. + 5. Shuts down the ROS2 communication. + + 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 517b0ee84..69539e8a5 100755 --- a/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py +++ b/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py @@ -5,6 +5,17 @@ def generate_launch_description(): + """ + 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 + includes a Node for the acoustics_data_record package. + + Returns: + LaunchDescription: A launch description containing the node configuration + for acoustics_data_record. + """ # Path to the YAML file yaml_file_path = os.path.join( get_package_share_directory("acoustics_interface"), 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 abe9a136d..a1aea9c49 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -79,6 +79,15 @@ def convertPandasObjectToIntArray(pandasObject): + """ + 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. + """ pandasString = pandasObject.iloc[0].strip("array('i', ").rstrip(")") pandasIntArray = [int(x.strip()) for x in pandasString.strip("[]").split(",")] @@ -86,6 +95,15 @@ def convertPandasObjectToIntArray(pandasObject): def convertPandasObjectToFloatArray(pandasObject): + """ + 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. + """ pandasString = pandasObject.iloc[0].strip("array('f', ").rstrip(")") pandasFloatArray = [float(x.strip()) for x in pandasString.strip("[]").split(",")] @@ -93,6 +111,32 @@ def convertPandasObjectToFloatArray(pandasObject): def getAcousticsData(): + """ + 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 + position data. The processed data is then returned in a structured format. + + Returns: + list: A list containing the following processed data: + - hydrophone1 (list of int): Data from Hydrophone 1. + - hydrophone2 (list of int): Data from Hydrophone 2. + - hydrophone3 (list of int): Data from Hydrophone 3. + - hydrophone4 (list of int): Data from Hydrophone 4. + - hydrophone5 (list of int): Data from Hydrophone 5. + - unfilteredData (list of int): Unfiltered data, same as the first 1024 values of Hydrophone 1. + - filteredData (list of int): Filtered response data. + - FFTAmplitudeData (list of int): Amplitude data from FFT. + - FFTFrequencyData (list of float): Frequency data corresponding to FFT amplitudes. + - peaksAmplitudeData (list of int): Amplitude data of peaks. + - peaksFrequencyData (list of int): Frequency data of peaks. + - tdoaData (list of float): Time Difference of Arrival (TDOA) data. + - positonData (list of float): Position data. + + 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] * hydrophoneDataSize hydrophone2 = [0] * hydrophoneDataSize @@ -189,6 +233,32 @@ def getAcousticsData(): def display_live_data(frame): + """ + Display live acoustics data by plotting hydrophone data, filter response, and FFT data. + + Args: + frame: The current frame for the live display (not used in the function). + + 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. + Plots the hydrophone data, filter response, and FFT data using predefined axes and colors. + Also prints out unused multilateration data (TDOA and position data). + + Acoustics data structure: + - acousticsData[0-4]: Hydrophone data for hydrophones 1 to 5 + - acousticsData[5]: Unfiltered data + - acousticsData[6]: Filtered data + - acousticsData[7]: FFT amplitude data + - acousticsData[8]: FFT frequency data + - acousticsData[9]: Peaks amplitude data + - acousticsData[10]: Peaks frequency data + - acousticsData[11]: TDOA data (currently not in use) + - acousticsData[12]: Position data (currently not in use) + + Note: + This function assumes that `getAcousticsData`, `hydrophoneAxis`, `filterAxis`, `FFTAxis`, + `colorSoftBlue`, `colorSoftGreen`, and `colorSoftPurple` are defined elsewhere in the code. + """ # Get latest acoustics data acousticsData = getAcousticsData() diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py index f474c11db..45494fe21 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py @@ -102,6 +102,12 @@ def __init__(self) -> None: self.get_logger().info("Sucsefully connected to Acoustics PCB MCU :D") def data_update(self) -> None: + """ + Fetches data using the TeensyCommunicationUDP class. + + This method calls the fetch_data method from the TeensyCommunicationUDP class + to update the data. + """ TeensyCommunicationUDP.fetch_data() def data_publisher(self) -> None: @@ -143,6 +149,16 @@ def data_publisher(self) -> None: def main(args=None): + """ + 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. + Once the node is shut down, 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. + """ rclpy.init(args=args) node = AcousticsInterfaceNode() diff --git a/acoustics/acoustics_interface/launch/acoustics_interface_launch.py b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py index 92e3b0b20..3c492c3de 100755 --- a/acoustics/acoustics_interface/launch/acoustics_interface_launch.py +++ b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py @@ -5,6 +5,17 @@ def generate_launch_description(): + """ + 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 + includes the node with the specified parameters. + + 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( get_package_share_directory("acoustics_interface"), diff --git a/auv_setup/launch/orca.launch.py b/auv_setup/launch/orca.launch.py index 2f56245a1..ec80e4ce9 100755 --- a/auv_setup/launch/orca.launch.py +++ b/auv_setup/launch/orca.launch.py @@ -7,6 +7,18 @@ def generate_launch_description(): + """ + 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 + interface components of the AUV. + + Returns: + 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}" diff --git a/auv_setup/launch/topside.launch.py b/auv_setup/launch/topside.launch.py index b3679eaeb..ab3e5fa37 100755 --- a/auv_setup/launch/topside.launch.py +++ b/auv_setup/launch/topside.launch.py @@ -7,6 +7,17 @@ def generate_launch_description(): + """ + 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 + includes the joystick interface launch description. + + 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}" diff --git a/mission/LCD/sources/IP_lib.py b/mission/LCD/sources/IP_lib.py index a71d029c5..2d5563d1f 100644 --- a/mission/LCD/sources/IP_lib.py +++ b/mission/LCD/sources/IP_lib.py @@ -8,6 +8,12 @@ def __init__(self): self.cmd = "hostname -I | cut -d' ' -f1" def get_IP(self): + """ + Executes a shell command to retrieve the IP address. + + Returns: + str: The IP address as a string. + """ IP_bytes = subprocess.check_output(self.cmd, shell=True) IP_str = IP_bytes.decode("utf-8") diff --git a/mission/LCD/sources/LCD.py b/mission/LCD/sources/LCD.py index 41f6f7fa9..0028279f2 100644 --- a/mission/LCD/sources/LCD.py +++ b/mission/LCD/sources/LCD.py @@ -19,6 +19,16 @@ # Formating function for nices LCD screen layout def format_line(value: str, unit: str): + """ + Formats a string to fit within a 16-character display, appending a unit with spacing. + + Args: + value (str): The value to be displayed. + unit (str): The unit to be appended to the value. + + Returns: + str: A formatted string that fits within a 16-character limit, with the unit appended. + """ spacesAvailable = 16 valueLenght = len(value) unitLenght = ( diff --git a/mission/LCD/sources/LCD_lib.py b/mission/LCD/sources/LCD_lib.py index 2d28466cd..20148e5ef 100644 --- a/mission/LCD/sources/LCD_lib.py +++ b/mission/LCD/sources/LCD_lib.py @@ -24,6 +24,19 @@ def __init__(self): ) def write_to_screen(self, line1: str = "", line2: str = ""): + """ + 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 + characters to ensure it fits on the screen. + + Args: + line1 (str): The text to display on the first line of the LCD screen. + 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() # limit line size as to big of a line destroys the view @@ -35,6 +48,23 @@ def write_to_screen(self, line1: str = "", line2: str = ""): self._LCD.write_string(line2) def fancy_animation(self, animation_speed=0.4): + """ + 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. + + The animation consists of two sequences: + 1. Ghost chasing Pac-Man from left to right. + 2. Pac-Man chasing the ghost from right to left. + + Custom characters used: + - Pac-Man with mouth open (left and right facing) + - Pac-Man with mouth closed + - Ghost + + The animation is displayed in two rows of the LCD screen. + """ # Calculate the apropriate animation speed animation_speed = 1 / animation_speed diff --git a/mission/LCD/sources/power_sense_module_lib.py b/mission/LCD/sources/power_sense_module_lib.py index fd9400e5a..095bfcdb0 100755 --- a/mission/LCD/sources/power_sense_module_lib.py +++ b/mission/LCD/sources/power_sense_module_lib.py @@ -37,6 +37,17 @@ def __init__(self): self.psm_to_battery_current_offset = 0.330 # V def get_voltage(self): + """ + 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 + I/O timeout or error occurs during the read operation, the method catches the + exception, logs an error message, and returns a default voltage value of 0.0. + + 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 = ( @@ -48,6 +59,16 @@ def get_voltage(self): return 0.0 def get_current(self): + """ + 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. + + Raises: + Exception: If there is an error in reading or converting the current. + """ try: system_current = ( self.channel_current.convert_and_read() diff --git a/mission/LCD/sources/pressure_sensor_lib.py b/mission/LCD/sources/pressure_sensor_lib.py index b1c226732..4b617936b 100755 --- a/mission/LCD/sources/pressure_sensor_lib.py +++ b/mission/LCD/sources/pressure_sensor_lib.py @@ -29,6 +29,12 @@ def __init__(self): time.sleep(1) def get_pressure(self): + """ + 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 return pressure diff --git a/mission/LCD/sources/temperature_sensor_lib.py b/mission/LCD/sources/temperature_sensor_lib.py index 3925fed4a..8484ddec9 100755 --- a/mission/LCD/sources/temperature_sensor_lib.py +++ b/mission/LCD/sources/temperature_sensor_lib.py @@ -16,6 +16,15 @@ def __init__(self): self.temperature_sensor_file_location = "/sys/class/thermal/thermal_zone0/temp" def get_temperature(self): + """ + 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 result = subprocess.run( diff --git a/mission/blackbox/blackbox/blackbox_log_data.py b/mission/blackbox/blackbox/blackbox_log_data.py index ac6976234..1ce465322 100755 --- a/mission/blackbox/blackbox/blackbox_log_data.py +++ b/mission/blackbox/blackbox/blackbox_log_data.py @@ -53,9 +53,26 @@ def __init__(self, ROS2_PACKAGE_DIRECTORY=""): writer.writerow(self.csv_headers) # Methods for inside use of the class ---------- - def manage_csv_files( - self, max_file_age_in_days=7, max_size_kb=3_000_000 - ): # adjust the max size before you start deleting old files (1 000 000 kb = 1 000 mb = 1 gb) + def manage_csv_files(self, max_file_age_in_days=7, max_size_kb=3_000_000): + """ + 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. + max_size_kb (int, optional): The maximum total size of all CSV files in kilobytes before the oldest files are deleted. Defaults to 3,000,000 KB (3 GB). + + Returns: + None + + Raises: + ValueError: If there is an error parsing the file timestamp. + + Notes: + - 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) @@ -165,6 +182,32 @@ def log_data_to_csv_file( pwm_7=0, pwm_8=0, ): + """ + 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 miliseconds 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 077fd0f7c..000526792 100755 --- a/mission/blackbox/blackbox/blackbox_node.py +++ b/mission/blackbox/blackbox/blackbox_node.py @@ -92,24 +92,100 @@ def __init__(self): # Callback Methods ---------- def psm_current_callback(self, msg): + """ + 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 + state with the latest current data. + + Args: + msg (std_msgs.msg.Float32): The message containing the current data. + """ self.psm_current_data = msg.data def psm_voltage_callback(self, msg): + """ + 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 + state with the latest voltage data. + + Args: + msg (std_msgs.msg.Float32): The message containing the voltage data. + """ self.psm_voltage_data = msg.data def pressure_callback(self, msg): + """ + 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 + pressure data. + + Args: + msg (std_msgs.msg.Float32): The message containing the pressure data. + """ self.pressure_data = msg.data def temperature_callback(self, msg): + """ + 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 + temperature data. + + Args: + msg (std_msgs.msg.Float32): The message containing the temperature data. + """ self.temperature_data = msg.data def thruster_forces_callback(self, msg): + """ + 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 + latest thruster forces data. + + Args: + msg (vortex_msgs.msg.ThrusterForces): The message containing the thruster forces data. + """ self.thruster_forces_data = msg.thrust def pwm_callback(self, msg): + """ + 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): + """ + 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 + temperature, thruster forces, and pulse-width modulation (PWM) signals. + It then logs this data to a CSV file using the `log_data_to_csv_file` method + of the `blackbox_log_data` object. + + The data logged includes: + - psm_current: Current data from the power system module. + - psm_voltage: Voltage data from the power system module. + - pressure_internal: Internal pressure data. + - temperature_ambient: Ambient temperature data. + - thruster_forces_1 to thruster_forces_8: Forces data from eight thrusters. + - pwm_1 to pwm_8: PWM signal data for eight channels. + """ self.blackbox_log_data.log_data_to_csv_file( psm_current=self.psm_current_data, psm_voltage=self.psm_voltage_data, @@ -135,6 +211,16 @@ def logger(self): def main(args=None): + """ + 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 + the node and shuts down the ROS2 environment. + + 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 3f675d368..a771b49f3 100644 --- a/mission/blackbox/launch/blackbox.launch.py +++ b/mission/blackbox/launch/blackbox.launch.py @@ -5,6 +5,16 @@ def generate_launch_description(): + """ + 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 + blackbox node with the specified parameters. + + Returns: + LaunchDescription: A launch description object containing the blackbox node. + """ # Path to the YAML file yaml_file_path = os.path.join( get_package_share_directory("blackbox"), 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 fd9400e5a..c3132f418 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 @@ -37,6 +37,18 @@ def __init__(self): self.psm_to_battery_current_offset = 0.330 # V def get_voltage(self): + """ + 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: + -------- + 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 = ( @@ -48,6 +60,18 @@ def get_voltage(self): return 0.0 def get_current(self): + """ + 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: + -------- + float + The current value in amperes. If an error occurs during reading, returns 0.0. + """ try: system_current = ( self.channel_current.convert_and_read() 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 128d79d38..01829622a 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,6 +70,12 @@ def __init__(self): self.get_logger().info('"power_sense_module_publisher" has been started') def read_timer_callback(self): + """ + 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. + """ # Get the PSM data self.current = self.PSM.get_current() self.voltage = self.PSM.get_voltage() @@ -89,7 +95,12 @@ def read_timer_callback(self): ) # publish voltage value to the "voltge topic" def warning_timer_callback(self): - # Check if Voltage is abnormal and if so print a warning + """ + 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. + """ if self.voltage < self.voltageMin: self.logger.fatal(f"WARNING: Battery Voltage to LOW at {self.voltage} V") elif self.voltage > self.voltageMax: @@ -97,6 +108,18 @@ def warning_timer_callback(self): def main(args=None): + """ + 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) power_sense_module_publisher = PowerSenseModulePublisher() 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 b1c226732..ccd1220af 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 @@ -29,6 +29,18 @@ def __init__(self): time.sleep(1) def get_pressure(self): + """ + 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: + -------- + float + The pressure reading in hPa (hectopascals), or 0.0 if an error occurs. + """ try: pressure = self.channel_pressure.pressure return 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 4e48e270d..f3caeadc2 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,6 +59,12 @@ def __init__(self): self.get_logger().info('"pressure_sensor_publisher" has been started') def timer_callback(self): + """ + Callback function triggered by the main timer. + + This function retrieves the pressure data from the sensor + and publishes it to the "/auv/pressure" topic. + """ # Get pressure data self.pressure = self.Pressure.get_pressure() @@ -68,7 +74,12 @@ def timer_callback(self): self.publisher_pressure.publish(pressure_msg) def warning_timer_callback(self): - # Check if Pressure is abnormaly to high, if so print a warning + """ + 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.pressureCriticalLevel: self.logger.fatal( f"WARNING: Internal pressure to HIGH: {self.pressure} hPa! Drone might be LEAKING!" @@ -76,6 +87,18 @@ def warning_timer_callback(self): def main(args=None): + """ + 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) pressure_publisher = PressurePublisher() 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 3925fed4a..467c33d97 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 @@ -16,6 +16,17 @@ def __init__(self): self.temperature_sensor_file_location = "/sys/class/thermal/thermal_zone0/temp" def get_temperature(self): + """ + 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: + -------- + float + The current temperature in Celsius. If an error occurs, it returns 0.0. + """ try: # Read internal temperature on the computer result = subprocess.run( 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 43f93f096..f4d1492da 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,6 +63,12 @@ def __init__(self): self.get_logger().info('"temperature_sensor_publisher" has been started') def timer_callback(self): + """ + Callback function triggered by the main timer. + + This function retrieves the temperature data from the sensor + and publishes it to the "/auv/temperature" topic. + """ # Get temperature data self.temperature = self.Temperature.get_temperature() @@ -72,7 +78,12 @@ def timer_callback(self): self.publisher_temperature.publish(temperature_msg) def warning_timer_callback(self): - # Check if Temperature is abnormal and if so print a warning + """ + 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.temperatureCriticalLevel: self.logger.fatal( f"WARNING: Temperature inside the Drone to HIGH: {self.temperature} *C! Drone might be overheating!" @@ -80,6 +91,18 @@ def warning_timer_callback(self): def main(args=None): + """ + 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) temperature_publisher = TemperaturePublisher() 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 7921797d5..610bb5fa3 100644 --- a/mission/internal_status_auv/launch/internal_status_auv.launch.py +++ b/mission/internal_status_auv/launch/internal_status_auv.launch.py @@ -5,6 +5,22 @@ def generate_launch_description(): + """ + 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. + Each node is configured using parameters from a YAML file located in the + auv_setup directory. + + The nodes will be launched with their respective executables and display + output on the screen. + + Returns: + -------- + launch.LaunchDescription + A LaunchDescription object containing the nodes to be launched. + """ # Path to the YAML file yaml_file_path = os.path.join( get_package_share_directory("internal_status_auv"), 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 e1f553196..d721befec 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 @@ -323,6 +323,13 @@ def joystick_cb(self, msg: Joy) -> Wrench: def main(): + """ + 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. + """ rclpy.init() joystick_interface = JoystickInterface() rclpy.spin(joystick_interface) 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 cd7c7563d..75958169d 100755 --- a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py +++ b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py @@ -5,6 +5,17 @@ def generate_launch_description(): + """ + 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 + parameters specified in the 'param_joystick_interface_auv.yaml' file. + + Returns: + LaunchDescription: A ROS 2 launch description containing the + joystick_interface_auv node. + """ joystick_interface_node = Node( package="joystick_interface_auv", executable="joystick_interface_auv.py", 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 ca67c1449..20c5f689d 100644 --- a/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py +++ b/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py @@ -8,6 +8,22 @@ class TestJoystickInterface: # test that the wrench msg is created successfully def test_wrench_msg(self): + """ + 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 + asserts that the message fields are correctly set. Finally, it shuts down + the ROS 2 client library. + + Assertions: + - The force.x field of the message is set to 2.0. + - The force.y field of the message is set to 3.0. + - The force.z field of the message is set to 4.0. + - The torque.x field of the message is set to 5.0. + - The torque.y field of the message is set to 6.0. + - The torque.z field of the message is set to 7.0. + """ rclpy.init() msg = JoystickInterface().create_wrench_message(2.0, 3.0, 4.0, 5.0, 6.0, 7.0) assert msg.force.x == 2.0 @@ -20,6 +36,27 @@ def test_wrench_msg(self): # Test that the callback function will be able to interpret the joy msg def test_input_from_controller_into_wrench_msg(self): + """ + 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 + function of the JoystickInterface class correctly converts the joystick + input into a Wrench message with the expected force and torque values. + + Assertions: + - The x component of the force in the wrench message should be the + negative of the first axis value scaled by joystick_surge_scaling_. + - The y component of the force in the wrench message should be the + second axis value scaled by joystick_sway_scaling_. + - The z component of the force in the wrench message should be 0.0. + - The x, y, and z components of the torque in the wrench message should + all be 0.0. + + 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() joy_msg.axes = [-1.0, -1.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0] @@ -35,6 +72,25 @@ def test_input_from_controller_into_wrench_msg(self): # When the killswitch button is activated in the buttons list, it should output a wrench msg with only zeros def test_killswitch_button(self): + """ + 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 + message with specific axes and button values to simulate pressing the + killswitch button. The joystick callback is invoked with this Joy message, + and the resulting wrench message is checked to ensure that all force and + torque components are zero, indicating that the killswitch has been + activated. Finally, the ROS 2 client library is shut down. + + Assertions: + - wrench_msg.force.x == 0.0 + - wrench_msg.force.y == 0.0 + - wrench_msg.force.z == 0.0 + - wrench_msg.torque.x == 0.0 + - wrench_msg.torque.y == 0.0 + - wrench_msg.torque.z == 0.0 + """ rclpy.init() joystick = JoystickInterface() joystick.state_ = States.XBOX_MODE @@ -52,6 +108,25 @@ def test_killswitch_button(self): # When we move into XBOX mode it should still be able to return this wrench message def test_moving_in_of_xbox_mode(self): + """ + 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 + message with specific axes and button values to simulate joystick input. + The joystick callback function is called with this Joy message, and the + resulting wrench message is checked to ensure that the force and torque + values are correctly calculated based on the joystick input and scaling + factors. + + Assertions: + - The x-component of the force should be the negative surge scaling factor. + - The y-component of the force should be the positive sway scaling factor. + - The z-component of the force should be zero. + - The x-component of the torque should be zero. + - The y-component of the torque should be zero. + - The z-component of the torque should be zero. + """ rclpy.init() joystick = JoystickInterface() joystick.state_ = States.XBOX_MODE 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 8d24d75bf..443f5e41e 100644 --- a/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py +++ b/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py @@ -5,6 +5,19 @@ def generate_launch_description(): + """ + 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 the 'orca.yaml' file located in the 'auv_setup' package's + 'config/robots' directory. The output of the node is set to be + displayed on the screen. + + Returns: + LaunchDescription: A ROS 2 LaunchDescription object containing + the thrust_allocator_auv_node. + """ thrust_allocator_auv_node = Node( package="thrust_allocator_auv", executable="thrust_allocator_auv_node", 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..b7fdcbacf 100644 --- a/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py +++ b/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py @@ -5,6 +5,18 @@ def generate_launch_description(): + """ + 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 + 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", 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..deea7b0f6 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 @@ -103,6 +103,17 @@ def _timer_callback(self): def main(args=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) From d6766cf8273211a2d22059dd9ef5ad5b13b6399f Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 17:18:37 +0200 Subject: [PATCH 04/51] refactor: fix pylint warning C0411 --- .../acoustics_data_record_lib.py | 2 +- .../acoustics_data_record_node.py | 9 ++++----- .../launch/acoustics_data_record.launch.py | 5 +++-- .../utilities/display_acoustics_data_live.py | 13 ++++++------- .../acoustics_interface/acoustics_interface_lib.py | 7 ++++--- .../acoustics_interface/acoustics_interface_node.py | 4 ++-- .../launch/acoustics_interface_launch.py | 5 +++-- auv_setup/launch/orca.launch.py | 6 +++--- auv_setup/launch/topside.launch.py | 5 +++-- mission/LCD/sources/LCD.py | 2 +- mission/LCD/sources/power_sense_module_lib.py | 1 + mission/LCD/sources/pressure_sensor_lib.py | 2 +- mission/blackbox/blackbox/blackbox_log_data.py | 2 +- mission/blackbox/blackbox/blackbox_node.py | 9 ++++----- mission/blackbox/launch/blackbox.launch.py | 5 +++-- .../internal_status_auv/power_sense_module_lib.py | 1 + .../internal_status_auv/power_sense_module_node.py | 7 +++---- .../internal_status_auv/pressure_sensor_lib.py | 2 +- .../internal_status_auv/pressure_sensor_node.py | 7 +++---- .../internal_status_auv/temperature_sensor_node.py | 7 +++---- .../launch/internal_status_auv.launch.py | 5 +++-- .../joystick_interface_auv.py | 5 ++--- .../launch/joystick_interface_auv.launch.py | 3 ++- .../tests/test_joystick_interface_auv.py | 5 ++--- .../launch/thrust_allocator_auv.launch.py | 1 + .../launch/thruster_interface_auv.launch.py | 5 +++-- .../thruster_interface_auv_driver_lib.py | 4 ++-- .../thruster_interface_auv_node.py | 8 +++----- 28 files changed, 69 insertions(+), 68 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 315c20239..bd863301c 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 @@ -1,6 +1,6 @@ # Python Libraries -import time import csv +import time from datetime import datetime 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 71d5d2b64..86d349dd8 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 @@ -1,16 +1,15 @@ #!/usr/bin/env python3 # ROS2 libraries -import rclpy -from rclpy.node import Node -from std_msgs.msg import Float32MultiArray, Int32MultiArray -from ament_index_python.packages import get_package_share_directory - # Python libraries import array +import rclpy # Custom libraries from acoustics_data_record_lib import AcousticsDataRecordLib +from ament_index_python.packages import get_package_share_directory +from rclpy.node import Node +from std_msgs.msg import Float32MultiArray, Int32MultiArray class AcousticsDataRecordNode(Node): 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 69539e8a5..548b3e65a 100755 --- a/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py +++ b/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py @@ -1,7 +1,8 @@ +import os + +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 -import os def generate_launch_description(): 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 a1aea9c49..bcb516eec 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -1,20 +1,19 @@ #!/usr/bin/env python3 # Libraries for file manipulation -import os -import sys +import array import ast import glob - -# Libraries for handling data structures -import pandas as pd -import numpy as np -import array +import os +import sys # Libraries for anmation import matplotlib.animation as animation import matplotlib.gridspec as gridspec import matplotlib.pyplot as plt +import numpy as np +# Libraries for handling data structures +import pandas as pd # Variables for seting upp data structures correctly hydrophoneDataSize = ( diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index aec0ded6e..89da82033 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -1,11 +1,12 @@ # Setting up libraries +import errno import os import sys +import time +from enum import Enum from socket import * + import netifaces as ni -from enum import Enum -import errno -import time class TeensyCommunicationUDP: diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py index 45494fe21..5dd8ff77d 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py @@ -1,10 +1,10 @@ #!/usr/bin/python3 import rclpy -from rclpy.node import Node import rclpy.logging -from std_msgs.msg import Int32MultiArray, Float32MultiArray from acoustics_interface.acoustics_interface_lib import TeensyCommunicationUDP +from rclpy.node import Node +from std_msgs.msg import Float32MultiArray, Int32MultiArray class AcousticsInterfaceNode(Node): diff --git a/acoustics/acoustics_interface/launch/acoustics_interface_launch.py b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py index 3c492c3de..91f3da639 100755 --- a/acoustics/acoustics_interface/launch/acoustics_interface_launch.py +++ b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py @@ -1,7 +1,8 @@ +import os + +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 -import os def generate_launch_description(): diff --git a/auv_setup/launch/orca.launch.py b/auv_setup/launch/orca.launch.py index ec80e4ce9..1d74c83fc 100755 --- a/auv_setup/launch/orca.launch.py +++ b/auv_setup/launch/orca.launch.py @@ -1,9 +1,9 @@ import os + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.actions import SetEnvironmentVariable, IncludeLaunchDescription +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable from launch.launch_description_sources import PythonLaunchDescriptionSource -from ament_index_python.packages import get_package_share_directory def generate_launch_description(): diff --git a/auv_setup/launch/topside.launch.py b/auv_setup/launch/topside.launch.py index ab3e5fa37..63e2c4fe4 100755 --- a/auv_setup/launch/topside.launch.py +++ b/auv_setup/launch/topside.launch.py @@ -1,9 +1,10 @@ import os + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import SetEnvironmentVariable, IncludeLaunchDescription +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory def generate_launch_description(): diff --git a/mission/LCD/sources/LCD.py b/mission/LCD/sources/LCD.py index 0028279f2..0853aa931 100644 --- a/mission/LCD/sources/LCD.py +++ b/mission/LCD/sources/LCD.py @@ -2,9 +2,9 @@ # Python Libraries from time import sleep +from IP_lib import IPDriver # Custom Libraries from LCD_lib import LCDScreenDriver -from IP_lib import IPDriver from power_sense_module_lib import PowerSenseModule from pressure_sensor_lib import PressureSensor from temperature_sensor_lib import TemperatureSensor diff --git a/mission/LCD/sources/power_sense_module_lib.py b/mission/LCD/sources/power_sense_module_lib.py index 095bfcdb0..e42efb969 100755 --- a/mission/LCD/sources/power_sense_module_lib.py +++ b/mission/LCD/sources/power_sense_module_lib.py @@ -1,5 +1,6 @@ # Libraies for Power Sense Module import time + import smbus from MCP342x import MCP342x diff --git a/mission/LCD/sources/pressure_sensor_lib.py b/mission/LCD/sources/pressure_sensor_lib.py index 4b617936b..ad93a6506 100755 --- a/mission/LCD/sources/pressure_sensor_lib.py +++ b/mission/LCD/sources/pressure_sensor_lib.py @@ -2,9 +2,9 @@ # Python Libraries import time +import adafruit_mprls # ! NOTE: sudo pip3 install adafruit-circuitpython-mprls # Pressure sensor Libraries import board -import adafruit_mprls # ! NOTE: sudo pip3 install adafruit-circuitpython-mprls class PressureSensor: diff --git a/mission/blackbox/blackbox/blackbox_log_data.py b/mission/blackbox/blackbox/blackbox_log_data.py index 1ce465322..e7e82c6e6 100755 --- a/mission/blackbox/blackbox/blackbox_log_data.py +++ b/mission/blackbox/blackbox/blackbox_log_data.py @@ -1,10 +1,10 @@ #!/usr/bin/env python3 # Python Libraries +import csv import os import re import time -import csv from datetime import datetime, timedelta diff --git a/mission/blackbox/blackbox/blackbox_node.py b/mission/blackbox/blackbox/blackbox_node.py index 000526792..219d7a966 100755 --- a/mission/blackbox/blackbox/blackbox_node.py +++ b/mission/blackbox/blackbox/blackbox_node.py @@ -1,17 +1,16 @@ #!/usr/bin/env python3 # ROS2 Libraries -import rclpy import array -from rclpy.node import Node -from ament_index_python.packages import get_package_share_directory +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 from std_msgs.msg import Float32, Int16MultiArray - # Custom Libraries from vortex_msgs.msg import ThrusterForces -from blackbox.blackbox_log_data import BlackBoxLogData class BlackBoxNode(Node): diff --git a/mission/blackbox/launch/blackbox.launch.py b/mission/blackbox/launch/blackbox.launch.py index a771b49f3..0b0af38ac 100644 --- a/mission/blackbox/launch/blackbox.launch.py +++ b/mission/blackbox/launch/blackbox.launch.py @@ -1,7 +1,8 @@ +import os + +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 -import os def generate_launch_description(): 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 c3132f418..505510938 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 @@ -1,5 +1,6 @@ # Libraies for Power Sense Module import time + import smbus from MCP342x import MCP342x 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 01829622a..bdfae7275 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,13 +1,12 @@ #!/usr/bin/env python3 # ROS2 Libraries +# Custom Libraries +import internal_status_auv.power_sense_module_lib import rclpy -from rclpy.node import Node from rclpy.logging import get_logger +from rclpy.node import Node from std_msgs.msg import Float32 -# Custom Libraries -import internal_status_auv.power_sense_module_lib - class PowerSenseModulePublisher(Node): def __init__(self): 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 ccd1220af..a077303c2 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 @@ -2,9 +2,9 @@ # Python Libraries import time +import adafruit_mprls # ! NOTE: sudo pip3 install adafruit-circuitpython-mprls # Pressure sensor Libraries import board -import adafruit_mprls # ! NOTE: sudo pip3 install adafruit-circuitpython-mprls class PressureSensor: 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 f3caeadc2..24cc718bf 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,13 +1,12 @@ #!/usr/bin/env python3 # ROS2 Libraries +# Custom Libraries +import internal_status_auv.pressure_sensor_lib import rclpy -from rclpy.node import Node from rclpy.logging import get_logger +from rclpy.node import Node from std_msgs.msg import Float32 -# Custom Libraries -import internal_status_auv.pressure_sensor_lib - class PressurePublisher(Node): def __init__(self): 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 f4d1492da..f8d25d45b 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,13 +1,12 @@ #!/usr/bin/python3 # ROS2 Libraries +# Custom Libraries +import internal_status_auv.temperature_sensor_lib import rclpy -from rclpy.node import Node from rclpy.logging import get_logger +from rclpy.node import Node from std_msgs.msg import Float32 -# Custom Libraries -import internal_status_auv.temperature_sensor_lib - class TemperaturePublisher(Node): def __init__(self): 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 610bb5fa3..b9e50229f 100644 --- a/mission/internal_status_auv/launch/internal_status_auv.launch.py +++ b/mission/internal_status_auv/launch/internal_status_auv.launch.py @@ -1,7 +1,8 @@ +import os + +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 -import os def generate_launch_description(): 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 d721befec..ed5390b6e 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 @@ -1,10 +1,9 @@ #!/usr/bin/python3 import rclpy -from rclpy.node import Node from geometry_msgs.msg import Wrench +from rclpy.node import Node from sensor_msgs.msg import Joy -from std_msgs.msg import Bool -from std_msgs.msg import String +from std_msgs.msg import Bool, String class States: 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 75958169d..db99b02ef 100755 --- a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py +++ b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py @@ -1,7 +1,8 @@ import os + +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 def generate_launch_description(): 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 20c5f689d..4a2ae93ee 100644 --- a/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py +++ b/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py @@ -1,7 +1,6 @@ -from joystick_interface_auv.joystick_interface_auv import JoystickInterface -from joystick_interface_auv.joystick_interface_auv import States import rclpy -from sensor_msgs.msg import Joy +from joystick_interface_auv.joystick_interface_auv import (JoystickInterface, + States) from sensor_msgs.msg import 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 443f5e41e..b7383ded5 100644 --- a/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py +++ b/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py @@ -1,4 +1,5 @@ from os import path + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node 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 b7fdcbacf..47805d804 100644 --- a/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py +++ b/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py @@ -1,7 +1,8 @@ +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(): 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..1a11e4c5e 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,8 +1,8 @@ #!/usr/bin/env python3 # Import libraries -import smbus2 -import pandas import numpy +import pandas +import smbus2 class ThrusterInterfaceAUVDriver: 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 deea7b0f6..40d3824c2 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,15 +1,13 @@ #!/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 ament_index_python.packages import get_package_share_directory - +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): From 9f9cd12215d8313e475d557e66f41a4f5aeb1b08 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 17:23:55 +0200 Subject: [PATCH 05/51] refactor: fix pylint warning R1731 --- mission/LCD/sources/LCD.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/LCD/sources/LCD.py b/mission/LCD/sources/LCD.py index 0853aa931..c4018a7f2 100644 --- a/mission/LCD/sources/LCD.py +++ b/mission/LCD/sources/LCD.py @@ -36,8 +36,8 @@ def format_line(value: str, unit: str): ) # +1 to make sure there is spacing between value and unit emptySpaceLenght = spacesAvailable - (valueLenght + unitLenght) - if emptySpaceLenght < 0: - emptySpaceLenght = 0 + emptySpaceLenght = max(emptySpaceLenght, 0) + formatedString = value[0 : (spacesAvailable - unitLenght)] formatedString += " " * emptySpaceLenght From 47f38a0b1d3c645267cfd471333d86ead70aae9d Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 17:26:53 +0200 Subject: [PATCH 06/51] refactor: fix pylint warning R1705 --- .../acoustics_interface_lib.py | 3 +-- .../joystick_interface_auv.py | 19 +++++++++---------- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index 89da82033..ea00ca6f1 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -197,8 +197,7 @@ def _parse_data_string(cls, is_float: bool) -> list[float] | list[int] | None: # Format data from CSV string to floats, ignore last value if is_float: return list(map(float, cls._data_string.split(",")[:-1])) - else: - return list(map(int, cls._data_string.split(",")[:-1])) + return list(map(int, cls._data_string.split(",")[:-1])) except Exception as e: print(f"The string '{cls._data_string}' caused an error when parsing") print(f"The exception was: {e}") 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 ed5390b6e..2e8432c48 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 @@ -282,16 +282,15 @@ def joystick_cb(self, msg: Joy) -> Wrench: self.transition_to_xbox_mode() return - else: - self.get_logger().info("SW killswitch", throttle_duration_sec=1) - # Signal that killswitch is blocking - self.software_killswitch_signal_publisher_.publish(Bool(data=True)) - - # Publish a zero wrench message when killswitch is activated - 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.state_ = States.NO_GO - return wrench_msg + self.get_logger().info("SW killswitch", throttle_duration_sec=1) + # Signal that killswitch is blocking + self.software_killswitch_signal_publisher_.publish(Bool(data=True)) + + # Publish a zero wrench message when killswitch is activated + 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.state_ = States.NO_GO + return wrench_msg # Toggle precise maneuvering mode on and off if precise_manuevering_mode_button: From 9ba77241373ab34aa8df654b81ecbeaea33c2dd2 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 17:29:43 +0200 Subject: [PATCH 07/51] refactor: fix pylint warning W1514 --- .../acoustics_data_record/acoustics_data_record_lib.py | 4 ++-- mission/blackbox/blackbox/blackbox_log_data.py | 4 ++-- 2 files changed, 4 insertions(+), 4 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 bd863301c..14801835c 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,7 @@ def __init__(self, ROS2_PACKAGE_DIRECTORY=""): ] # Make new .csv file for loging blackbox data ---------- - with open(self.data_file_location, mode="w", newline="") 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) @@ -69,7 +69,7 @@ def log_data_to_csv_file( current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] # Write to .csv file - with open(self.data_file_location, mode="a", newline="") 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_log_data.py b/mission/blackbox/blackbox/blackbox_log_data.py index e7e82c6e6..145d46c77 100755 --- a/mission/blackbox/blackbox/blackbox_log_data.py +++ b/mission/blackbox/blackbox/blackbox_log_data.py @@ -48,7 +48,7 @@ def __init__(self, ROS2_PACKAGE_DIRECTORY=""): self.manage_csv_files() # Make new .csv file for loging blackbox data ---------- - with open(self.data_file_location, mode="w", newline="") 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) @@ -212,7 +212,7 @@ def log_data_to_csv_file( current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] # Write to .csv file - with open(self.data_file_location, mode="a", newline="") 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( [ From 90eacacad7dc1bd79b25a003b4ad5164f626283a Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 17:32:48 +0200 Subject: [PATCH 08/51] refactor: fix pylint warning C0200 --- mission/blackbox/blackbox/blackbox_log_data.py | 2 +- .../thruster_interface_auv_driver_lib.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/blackbox/blackbox/blackbox_log_data.py b/mission/blackbox/blackbox/blackbox_log_data.py index 145d46c77..5566b0ebe 100755 --- a/mission/blackbox/blackbox/blackbox_log_data.py +++ b/mission/blackbox/blackbox/blackbox_log_data.py @@ -59,7 +59,7 @@ def manage_csv_files(self, max_file_age_in_days=7, max_size_kb=3_000_000): Args: max_file_age_in_days (int, optional): The maximum age of files in days before they are deleted. Defaults to 7 days. - max_size_kb (int, optional): The maximum total size of all CSV files in kilobytes before the oldest files are deleted. Defaults to 3,000,000 KB (3 GB). + max_size_kb (int, optional): The maximum total size of all CSV files in kilobytes before the oldest files are deleted. Returns: None 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 1a11e4c5e..57622f86d 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 @@ -93,7 +93,7 @@ def _send_data_to_escs(self, thruster_pwm_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)): + 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]) @@ -135,7 +135,7 @@ def drive_thrusters(self, thruster_forces_array): ) # Apply thruster offset and limit PWM if needed - for ESC_channel in range(len(thruster_pwm_array)): + 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] From e4fb07f12ec5d808f1089506b387bdd126432095 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 17:36:13 +0200 Subject: [PATCH 09/51] refactor: fix pylint warning W0611 --- .../utilities/display_acoustics_data_live.py | 4 ---- .../acoustics_interface/acoustics_interface_lib.py | 7 ++----- 2 files changed, 2 insertions(+), 9 deletions(-) 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 bcb516eec..0a037e20e 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -1,17 +1,13 @@ #!/usr/bin/env python3 # Libraries for file manipulation -import array -import ast import glob import os -import sys # Libraries for anmation import matplotlib.animation as animation import matplotlib.gridspec as gridspec import matplotlib.pyplot as plt -import numpy as np # Libraries for handling data structures import pandas as pd diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index ea00ca6f1..3f925a3f9 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -1,12 +1,9 @@ # Setting up libraries import errno -import os -import sys import time -from enum import Enum -from socket import * +from socket import socket, AF_INET, SOCK_DGRAM, error + -import netifaces as ni class TeensyCommunicationUDP: From d4e3331eac223c87b08da8e022f28953ff016b6d Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 17:41:29 +0200 Subject: [PATCH 10/51] refactor: fix pylint warning W0702 --- .../utilities/display_acoustics_data_live.py | 10 ++++++---- .../acoustics_interface/acoustics_interface_lib.py | 4 ++-- 2 files changed, 8 insertions(+), 6 deletions(-) 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 0a037e20e..21012fd68 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -180,8 +180,9 @@ def getAcousticsData(): # Get multilateration data tdoaData = convertPandasObjectToFloatArray(latestAcousticsData["TDOA"]) positonData = convertPandasObjectToFloatArray(latestAcousticsData["Position"]) - except: - print("ERROR: Coulden't read acoustics data") + except Exception as e: + print(f"ERROR: Couldn't read acoustics data. Exception: {e}") + # Post process DSP data to desired scale and amount ---------- # 1. Convert FFTData to its corresponding frequency amount @@ -206,8 +207,9 @@ def getAcousticsData(): peaksAmplitudeData = tempAmplitude peaksFrequencyData = tempFrequency - except: - print("ERROR processing DSP data") + except Exception as e: + print(f"ERROR processing DSP data. Exception: {e}") + # return processed data ---------- return [ diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index 3f925a3f9..934c2cde7 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -285,5 +285,5 @@ def _send_frequencies_of_interest( # print(self.address); cls._clientSocket.sendto(frequency_variance_msg.encode(), cls._address) - except: - print("Couldn't send Frequency data") + except Exception as e: + print(f"Unexpected error while sending frequency data: {e}") From 8478f60d7a8dc92722f7cf6d1d916f78edf792ef Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 17:42:26 +0200 Subject: [PATCH 11/51] refactor: fix pylint warning C0121 --- .../acoustics_interface/acoustics_interface_lib.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index 934c2cde7..f8a5208ec 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -123,7 +123,7 @@ def fetch_data(cls) -> None: while True: data = cls._get_raw_data() - if data == None: + if data is None: return if data not in cls.acoustics_data.keys(): @@ -150,7 +150,7 @@ def _write_to_target(cls) -> None: else: data = cls._parse_data_string(is_float=False) - if data == None: + if data is None: cls._data_string = "" return @@ -246,7 +246,7 @@ def _check_if_available(cls) -> None: # Read data message = cls._get_raw_data() # Check if there is no more data left - if message == None: + if message is None: return False # Check if correct signal was sent From e49408b2fa01821cc9b5756c26908d74cfda74ee Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 17:43:41 +0200 Subject: [PATCH 12/51] refactor: fix pylint warning W0107 --- .../acoustics_interface/acoustics_interface_lib.py | 1 - .../acoustics_interface/acoustics_interface_node.py | 1 - 2 files changed, 2 deletions(-) diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index f8a5208ec..d505fc05e 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -230,7 +230,6 @@ def _send_acknowledge_signal(cls) -> None: except Exception as e: print("Error from send_acknowledge_signal") print(e) - pass @classmethod def _check_if_available(cls) -> None: diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py index 5dd8ff77d..bf668cbcc 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py @@ -167,7 +167,6 @@ def main(args=None): node.destroy_node() rclpy.shutdown() - pass if __name__ == "__main__": From f971a79438fbee66236024c86659e9ab675f931e Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 17:44:51 +0200 Subject: [PATCH 13/51] refactor: fix pylint warning R1714 --- .../acoustics_interface/acoustics_interface_lib.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index d505fc05e..db40dcfef 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -145,7 +145,7 @@ def _write_to_target(cls) -> None: """ Writes to the current target in `acoustics_data` and clears the data string """ - if cls._data_target == "TDOA" or cls._data_target == "LOCATION": + if cls._data_target in {"TDOA", "LOCATION"}: data = cls._parse_data_string(is_float=True) else: data = cls._parse_data_string(is_float=False) From 6953afcc416b73913df0af0aa11e0f427f0d845d Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 18:14:15 +0200 Subject: [PATCH 14/51] refactor: fix pylint warning C0201 --- .../acoustics_interface/acoustics_interface_lib.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index db40dcfef..7a6850304 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -126,7 +126,7 @@ def fetch_data(cls) -> None: if data is None: return - if data not in cls.acoustics_data.keys(): + if data not in cls.acoustics_data: cls._data_string += data else: cls._write_to_target() From 3e24960f0fd64efaebe1bd63dd9cbbcb4a1215e7 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 18:16:05 +0200 Subject: [PATCH 15/51] refactor: fix pylint warning C0303 and W0613 --- .../utilities/display_acoustics_data_live.py | 5 +---- .../acoustics_interface/acoustics_interface_lib.py | 2 +- 2 files changed, 2 insertions(+), 5 deletions(-) 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 21012fd68..ce5e4e614 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -229,13 +229,10 @@ def getAcousticsData(): ] -def display_live_data(frame): +def display_live_data(): """ Display live acoustics data by plotting hydrophone data, filter response, and FFT data. - Args: - frame: The current frame for the live display (not used in the function). - 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. Plots the hydrophone data, filter response, and FFT data using predefined axes and colors. diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index 7a6850304..b8537f413 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -284,5 +284,5 @@ def _send_frequencies_of_interest( # print(self.address); cls._clientSocket.sendto(frequency_variance_msg.encode(), cls._address) - except Exception as e: + except Exception as e: print(f"Unexpected error while sending frequency data: {e}") From 73f7e1de812474b075f8c3b0505a79bc0b7bfbff Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 18:19:38 +0200 Subject: [PATCH 16/51] refactor: fix pylint warning W0621 --- .../utilities/display_acoustics_data_live.py | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) 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 ce5e4e614..971b3f936 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -200,10 +200,10 @@ def getAcousticsData(): try: tempAmplitude = [] tempFrequency = [] - for i in range(1, len(peaksData), 3): - if peaksData[i] < MAX_FREQUENCY_TO_SHOW: - tempAmplitude += [peaksData[i - 1]] - tempFrequency += [peaksData[i]] + for peak_index in range(1, len(peaksData), 3): + if peaksData[peak_index] < MAX_FREQUENCY_TO_SHOW: + tempAmplitude += [peaksData[peak_index - 1]] + tempFrequency += [peaksData[peak_index]] peaksAmplitudeData = tempAmplitude peaksFrequencyData = tempFrequency @@ -277,17 +277,17 @@ def display_live_data(): positionData = acousticsData[12] # Currently not in use # Plot hydrophone data - for i in range(5): - xHydrophone = list(range(len(hydrophoneData[i][::]))) - hydrophoneAxis[i].clear() - hydrophoneAxis[i].plot( + for hydrophone_index in range(5): + xHydrophone = list(range(len(hydrophoneData[hydrophone_index][::]))) + hydrophoneAxis[hydrophone_index].clear() + hydrophoneAxis[hydrophone_index].plot( xHydrophone, - hydrophoneData[i], - label=f"Hydrophone {i + 1}", + hydrophoneData[hydrophone_index], + label=f"Hydrophone {hydrophone_index + 1}", color=colorSoftBlue, alpha=1, ) - hydrophoneAxis[i].legend(loc="upper right", fontsize="xx-small") + hydrophoneAxis[hydrophone_index].legend(loc="upper right", fontsize="xx-small") # Plot Filter response xRaw = list(range(len(unfilteredData))) From 6349e1e353a9aa93ff77955b6d2cdb05d101c891 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 18:21:48 +0200 Subject: [PATCH 17/51] refactor: fix pylint warning R0402 --- .../utilities/display_acoustics_data_live.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 971b3f936..8c7a2077a 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -5,8 +5,8 @@ import os # Libraries for anmation -import matplotlib.animation as animation -import matplotlib.gridspec as gridspec +from matplotlib import animation +from matplotlib import gridspec import matplotlib.pyplot as plt # Libraries for handling data structures import pandas as pd From 3296c3e221934a2c52613bae90316464f2f4ddc7 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 18:34:24 +0200 Subject: [PATCH 18/51] refactor: fix pylint warning W0212 --- .../joystick_interface_auv/joystick_interface_auv.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 2e8432c48..e8804c906 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 @@ -187,7 +187,7 @@ def joystick_cb(self, msg: Joy) -> Wrench: A ROS message containing the wrench data that was sent to the thruster allocation node. """ - current_time = self.get_clock().now().to_msg()._sec + current_time = self.get_clock().now().to_msg().sec buttons = {} axes = {} From 36bd73f7dec0eebccb746de4ecb58afb82b75b5a Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 18:35:09 +0200 Subject: [PATCH 19/51] chore(pylint): update rule adjustments and formatting --- .pylintrc | 50 ++++++++++++++++++++++++++++++-------------------- 1 file changed, 30 insertions(+), 20 deletions(-) diff --git a/.pylintrc b/.pylintrc index 454f365b0..973a0293c 100644 --- a/.pylintrc +++ b/.pylintrc @@ -299,7 +299,7 @@ max-attributes=22 max-bool-expr=5 # Maximum number of branch for function / method body. -max-branches=17 +max-branches=18 # Maximum number of locals for function / method body. max-locals=24 @@ -317,7 +317,7 @@ max-public-methods=20 max-returns=6 # Maximum number of statements in function / method body. -max-statements=63 +max-statements=64 # Minimum number of public methods for a class (see R0903). min-public-methods=2 @@ -430,24 +430,34 @@ confidence=HIGH, # no Warning level messages displayed, use "--disable=all --enable=classes # --disable=W". disable=raw-checker-failed, - bad-inline-option, - locally-disabled, - file-ignored, - suppressed-message, - useless-suppression, - deprecated-pragma, - use-symbolic-message-instead, - use-implicit-booleaness-not-comparison-to-string, - use-implicit-booleaness-not-comparison-to-zero, - invalid-name, # We need to fix this, but it takes too fucking long - import-error, # E0401: Unable to import - broad-exception-caught, # Someone who knows the code needs to fix this. - too-few-public-methods, # Someone needs to make this class into just two methods - duplicate-code, # Can be ignored, but someone might need to improve the code - dangerous-default-value, # Someone else who knows what they are doing needs to fix this - inconsistent-return-statements, # Need to ask someone who knows how to fix this - no-name-in-module, - unknown-option-value, + # 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 + invalid-name, # [C0103] Naming convention not followed (e.g., function, variable, or class names) + + # 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 From 44f683ee6e7aee80c3842e42cc4e34f6be137192 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 18:40:36 +0200 Subject: [PATCH 20/51] style: format code with black --- .../acoustics_data_record/acoustics_data_record_lib.py | 8 ++++++-- .../acoustics_data_record_node.py | 1 + .../utilities/display_acoustics_data_live.py | 3 +-- .../acoustics_interface/acoustics_interface_lib.py | 2 -- mission/LCD/sources/LCD.py | 2 +- mission/LCD/sources/pressure_sensor_lib.py | 1 + mission/blackbox/blackbox/blackbox_log_data.py | 10 +++++++--- mission/blackbox/blackbox/blackbox_node.py | 2 ++ .../internal_status_auv/pressure_sensor_lib.py | 1 + .../tests/test_joystick_interface_auv.py | 3 +-- .../thruster_interface_auv_node.py | 6 ++++-- 11 files changed, 25 insertions(+), 14 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 14801835c..d780bee26 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=""): ] # Make new .csv file for loging 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) @@ -69,7 +71,9 @@ def log_data_to_csv_file( current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] # Write to .csv file - with open(self.data_file_location, mode="a", newline="", encoding="utf-8") as csv_file: + with open( + self.data_file_location, mode="a", newline="", encoding="utf-8" + ) as csv_file: writer = csv.writer(csv_file) writer.writerow( [ diff --git a/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py b/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py index 86d349dd8..f4c0a3f34 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 @@ -5,6 +5,7 @@ import array import rclpy + # Custom libraries from acoustics_data_record_lib import AcousticsDataRecordLib from ament_index_python.packages import get_package_share_directory 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 8c7a2077a..caa65ce3e 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -8,6 +8,7 @@ from matplotlib import animation from matplotlib import gridspec import matplotlib.pyplot as plt + # Libraries for handling data structures import pandas as pd @@ -183,7 +184,6 @@ def getAcousticsData(): except Exception as e: print(f"ERROR: Couldn't read acoustics data. Exception: {e}") - # Post process DSP data to desired scale and amount ---------- # 1. Convert FFTData to its corresponding frequency amount # 2. Cut out big FFT frequencies out as they are not relevant @@ -210,7 +210,6 @@ def getAcousticsData(): except Exception as e: print(f"ERROR processing DSP data. Exception: {e}") - # return processed data ---------- return [ hydrophone1, diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index b8537f413..857ce9b36 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -4,8 +4,6 @@ from socket import socket, AF_INET, SOCK_DGRAM, error - - class TeensyCommunicationUDP: """ This class is responsible for the RPI side of teensy-RPI UDP communication. It is diff --git a/mission/LCD/sources/LCD.py b/mission/LCD/sources/LCD.py index c4018a7f2..1946e1afd 100644 --- a/mission/LCD/sources/LCD.py +++ b/mission/LCD/sources/LCD.py @@ -3,6 +3,7 @@ from time import sleep from IP_lib import IPDriver + # Custom Libraries from LCD_lib import LCDScreenDriver from power_sense_module_lib import PowerSenseModule @@ -38,7 +39,6 @@ def format_line(value: str, unit: str): emptySpaceLenght = spacesAvailable - (valueLenght + unitLenght) emptySpaceLenght = max(emptySpaceLenght, 0) - formatedString = value[0 : (spacesAvailable - unitLenght)] formatedString += " " * emptySpaceLenght formatedString += " " + unit diff --git a/mission/LCD/sources/pressure_sensor_lib.py b/mission/LCD/sources/pressure_sensor_lib.py index ad93a6506..55e990ea8 100755 --- a/mission/LCD/sources/pressure_sensor_lib.py +++ b/mission/LCD/sources/pressure_sensor_lib.py @@ -3,6 +3,7 @@ import time import adafruit_mprls # ! NOTE: sudo pip3 install adafruit-circuitpython-mprls + # Pressure sensor Libraries import board diff --git a/mission/blackbox/blackbox/blackbox_log_data.py b/mission/blackbox/blackbox/blackbox_log_data.py index 5566b0ebe..0cdbd9b10 100755 --- a/mission/blackbox/blackbox/blackbox_log_data.py +++ b/mission/blackbox/blackbox/blackbox_log_data.py @@ -48,7 +48,9 @@ def __init__(self, ROS2_PACKAGE_DIRECTORY=""): self.manage_csv_files() # Make new .csv file for loging 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) @@ -59,7 +61,7 @@ def manage_csv_files(self, max_file_age_in_days=7, max_size_kb=3_000_000): Args: max_file_age_in_days (int, optional): The maximum age of files in days before they are deleted. Defaults to 7 days. - max_size_kb (int, optional): The maximum total size of all CSV files in kilobytes before the oldest files are deleted. + max_size_kb (int, optional): The maximum total size of all CSV files in kilobytes before the oldest files are deleted. Returns: None @@ -212,7 +214,9 @@ def log_data_to_csv_file( current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] # Write to .csv file - with open(self.data_file_location, mode="a", newline="", encoding="utf-8") as csv_file: + with open( + self.data_file_location, mode="a", newline="", encoding="utf-8" + ) as csv_file: writer = csv.writer(csv_file) writer.writerow( [ diff --git a/mission/blackbox/blackbox/blackbox_node.py b/mission/blackbox/blackbox/blackbox_node.py index 219d7a966..921639216 100755 --- a/mission/blackbox/blackbox/blackbox_node.py +++ b/mission/blackbox/blackbox/blackbox_node.py @@ -7,8 +7,10 @@ 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 from std_msgs.msg import Float32, Int16MultiArray + # Custom Libraries from vortex_msgs.msg import ThrusterForces 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 a077303c2..53114d3e1 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 @@ -3,6 +3,7 @@ import time import adafruit_mprls # ! NOTE: sudo pip3 install adafruit-circuitpython-mprls + # Pressure sensor Libraries import board 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 4a2ae93ee..f0c504ac7 100644 --- a/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py +++ b/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py @@ -1,6 +1,5 @@ import rclpy -from joystick_interface_auv.joystick_interface_auv import (JoystickInterface, - States) +from joystick_interface_auv.joystick_interface_auv import JoystickInterface, States from sensor_msgs.msg import Joy 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 40d3824c2..cb878a232 100755 --- a/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py +++ b/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py @@ -4,8 +4,10 @@ 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 +from thruster_interface_auv.thruster_interface_auv_driver_lib import ( + ThrusterInterfaceAUVDriver, +) + # Custom libraries from vortex_msgs.msg import ThrusterForces From 5dae72156f895bd58dc7159260340546590cf8ee Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 18:41:19 +0200 Subject: [PATCH 21/51] style: format imports with isort --- .../acoustics_data_record/acoustics_data_record_node.py | 1 - .../utilities/display_acoustics_data_live.py | 6 ++---- .../acoustics_interface/acoustics_interface_lib.py | 2 +- mission/LCD/sources/LCD.py | 1 - mission/LCD/sources/pressure_sensor_lib.py | 1 - mission/blackbox/blackbox/blackbox_node.py | 2 -- .../internal_status_auv/pressure_sensor_lib.py | 1 - .../tests/test_joystick_interface_auv.py | 3 ++- .../thruster_interface_auv/thruster_interface_auv_node.py | 6 ++---- 9 files changed, 7 insertions(+), 16 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 f4c0a3f34..86d349dd8 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 @@ -5,7 +5,6 @@ import array import rclpy - # Custom libraries from acoustics_data_record_lib import AcousticsDataRecordLib from ament_index_python.packages import get_package_share_directory 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 caa65ce3e..4b4b9108d 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -4,13 +4,11 @@ import glob import os -# Libraries for anmation -from matplotlib import animation -from matplotlib import gridspec import matplotlib.pyplot as plt - # Libraries for handling data structures import pandas as pd +# Libraries for anmation +from matplotlib import animation, gridspec # Variables for seting upp data structures correctly hydrophoneDataSize = ( diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index 857ce9b36..a24f812f4 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -1,7 +1,7 @@ # Setting up libraries import errno import time -from socket import socket, AF_INET, SOCK_DGRAM, error +from socket import AF_INET, SOCK_DGRAM, error, socket class TeensyCommunicationUDP: diff --git a/mission/LCD/sources/LCD.py b/mission/LCD/sources/LCD.py index 1946e1afd..521c9f751 100644 --- a/mission/LCD/sources/LCD.py +++ b/mission/LCD/sources/LCD.py @@ -3,7 +3,6 @@ from time import sleep from IP_lib import IPDriver - # Custom Libraries from LCD_lib import LCDScreenDriver from power_sense_module_lib import PowerSenseModule diff --git a/mission/LCD/sources/pressure_sensor_lib.py b/mission/LCD/sources/pressure_sensor_lib.py index 55e990ea8..ad93a6506 100755 --- a/mission/LCD/sources/pressure_sensor_lib.py +++ b/mission/LCD/sources/pressure_sensor_lib.py @@ -3,7 +3,6 @@ import time import adafruit_mprls # ! NOTE: sudo pip3 install adafruit-circuitpython-mprls - # Pressure sensor Libraries import board diff --git a/mission/blackbox/blackbox/blackbox_node.py b/mission/blackbox/blackbox/blackbox_node.py index 921639216..219d7a966 100755 --- a/mission/blackbox/blackbox/blackbox_node.py +++ b/mission/blackbox/blackbox/blackbox_node.py @@ -7,10 +7,8 @@ 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 from std_msgs.msg import Float32, Int16MultiArray - # Custom Libraries from vortex_msgs.msg import ThrusterForces 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 53114d3e1..a077303c2 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 @@ -3,7 +3,6 @@ import time import adafruit_mprls # ! NOTE: sudo pip3 install adafruit-circuitpython-mprls - # Pressure sensor Libraries import board 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 f0c504ac7..4a2ae93ee 100644 --- a/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py +++ b/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py @@ -1,5 +1,6 @@ import rclpy -from joystick_interface_auv.joystick_interface_auv import JoystickInterface, States +from joystick_interface_auv.joystick_interface_auv import (JoystickInterface, + States) from sensor_msgs.msg import Joy 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 cb878a232..40d3824c2 100755 --- a/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py +++ b/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py @@ -4,10 +4,8 @@ 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, -) - +from thruster_interface_auv.thruster_interface_auv_driver_lib import \ + ThrusterInterfaceAUVDriver # Custom libraries from vortex_msgs.msg import ThrusterForces From 61a507cc7267a64d8c0db70616fbfcf3f2cfea1e Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 18:44:31 +0200 Subject: [PATCH 22/51] ci: add isort and pylint to Python pipeline --- .github/workflows/black-formatter.yml | 28 ----------- .github/workflows/python-format-lint.yml | 61 ++++++++++++++++++++++++ 2 files changed, 61 insertions(+), 28 deletions(-) delete mode 100644 .github/workflows/black-formatter.yml create mode 100644 .github/workflows/python-format-lint.yml diff --git a/.github/workflows/black-formatter.yml b/.github/workflows/black-formatter.yml deleted file mode 100644 index 1e3fd365c..000000000 --- a/.github/workflows/black-formatter.yml +++ /dev/null @@ -1,28 +0,0 @@ -name: Run black-format Linter - -on: [pull_request] - -jobs: - black_format: - runs-on: ubuntu-latest - - steps: - - uses: actions/checkout@v4 - with: - ref: ${{ github.head_ref }} - - - name: Check files using the black formatter - uses: rickstaa/action-black@v1 - id: action_black - with: - black_args: "." - - - name: Commit changes if code is formatted - if: steps.action_black.outputs.is_formatted == 'true' - uses: EndBug/add-and-commit@v9 - with: - author_name: Black Robot - author_email: black-robot@example.com - message: 'Committing black-format changes' - env: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} \ No newline at end of file diff --git a/.github/workflows/python-format-lint.yml b/.github/workflows/python-format-lint.yml new file mode 100644 index 000000000..61d36e1d0 --- /dev/null +++ b/.github/workflows/python-format-lint.yml @@ -0,0 +1,61 @@ +name: Run Python formatters and linters + +on: [push] + +jobs: + black_format: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v4 + with: + ref: ${{ github.head_ref }} + + - name: Check files using the black formatter + uses: rickstaa/action-black@v1 + id: action_black + with: + black_args: "." + + - name: Commit changes if code is formatted + uses: EndBug/add-and-commit@v9 + with: + author_name: Black Robot + author_email: black-robot@example.com + message: 'Committing black-format changes' + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + + isort_format: + runs-on: ubuntu-latest + needs: black_format + + steps: + - uses: actions/checkout@v4 + with: + ref: ${{ github.head_ref }} + + - name: Run isort to sort imports + run: isort . + + - name: Commit isort changes + uses: EndBug/add-and-commit@v9 + with: + author_name: Isort Robot + author_email: isort-robot@example.com + message: 'Committing isort changes' + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + + pylint_check: + runs-on: ubuntu-latest + needs: isort_format + + steps: + - uses: actions/checkout@v4 + with: + ref: ${{ github.head_ref }} + + - name: Run pylint + run: | + pylint . \ No newline at end of file From a0aa60e446cf6d589d85dbbb9f35f5498852c60d Mon Sep 17 00:00:00 2001 From: Black Robot Date: Mon, 23 Sep 2024 16:45:21 +0000 Subject: [PATCH 23/51] Committing black-format changes --- .../acoustics_data_record/acoustics_data_record_node.py | 1 + .../utilities/display_acoustics_data_live.py | 2 ++ mission/LCD/sources/LCD.py | 1 + mission/LCD/sources/pressure_sensor_lib.py | 1 + mission/blackbox/blackbox/blackbox_node.py | 2 ++ .../internal_status_auv/pressure_sensor_lib.py | 1 + .../tests/test_joystick_interface_auv.py | 3 +-- .../thruster_interface_auv/thruster_interface_auv_node.py | 6 ++++-- 8 files changed, 13 insertions(+), 4 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 86d349dd8..f4c0a3f34 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 @@ -5,6 +5,7 @@ import array import rclpy + # Custom libraries from acoustics_data_record_lib import AcousticsDataRecordLib from ament_index_python.packages import get_package_share_directory 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 4b4b9108d..39488581b 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -5,8 +5,10 @@ import os import matplotlib.pyplot as plt + # Libraries for handling data structures import pandas as pd + # Libraries for anmation from matplotlib import animation, gridspec diff --git a/mission/LCD/sources/LCD.py b/mission/LCD/sources/LCD.py index 521c9f751..1946e1afd 100644 --- a/mission/LCD/sources/LCD.py +++ b/mission/LCD/sources/LCD.py @@ -3,6 +3,7 @@ from time import sleep from IP_lib import IPDriver + # Custom Libraries from LCD_lib import LCDScreenDriver from power_sense_module_lib import PowerSenseModule diff --git a/mission/LCD/sources/pressure_sensor_lib.py b/mission/LCD/sources/pressure_sensor_lib.py index ad93a6506..55e990ea8 100755 --- a/mission/LCD/sources/pressure_sensor_lib.py +++ b/mission/LCD/sources/pressure_sensor_lib.py @@ -3,6 +3,7 @@ import time import adafruit_mprls # ! NOTE: sudo pip3 install adafruit-circuitpython-mprls + # Pressure sensor Libraries import board diff --git a/mission/blackbox/blackbox/blackbox_node.py b/mission/blackbox/blackbox/blackbox_node.py index 219d7a966..921639216 100755 --- a/mission/blackbox/blackbox/blackbox_node.py +++ b/mission/blackbox/blackbox/blackbox_node.py @@ -7,8 +7,10 @@ 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 from std_msgs.msg import Float32, Int16MultiArray + # Custom Libraries from vortex_msgs.msg import ThrusterForces 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 a077303c2..53114d3e1 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 @@ -3,6 +3,7 @@ import time import adafruit_mprls # ! NOTE: sudo pip3 install adafruit-circuitpython-mprls + # Pressure sensor Libraries import board 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 4a2ae93ee..f0c504ac7 100644 --- a/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py +++ b/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py @@ -1,6 +1,5 @@ import rclpy -from joystick_interface_auv.joystick_interface_auv import (JoystickInterface, - States) +from joystick_interface_auv.joystick_interface_auv import JoystickInterface, States from sensor_msgs.msg import Joy 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 40d3824c2..cb878a232 100755 --- a/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py +++ b/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py @@ -4,8 +4,10 @@ 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 +from thruster_interface_auv.thruster_interface_auv_driver_lib import ( + ThrusterInterfaceAUVDriver, +) + # Custom libraries from vortex_msgs.msg import ThrusterForces From 13c3e23d732965b7372a9ad2a078d5b182b008f6 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 18:53:40 +0200 Subject: [PATCH 24/51] chore: add pyproject.toml for project configuration --- pyproject.toml | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 pyproject.toml diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 000000000..3b64841c4 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,7 @@ +[tool.black] +line-length = 88 +target-version = ['py39'] +skip-string-normalization = true + +[tool.isort] +profile = "black" From 45e29195d23e25365aa5fc516b1f0f327d6db4ea Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 18:57:17 +0200 Subject: [PATCH 25/51] ci: update order of pipeline jobs --- .github/workflows/python-format-lint.yml | 45 +++++++++++++----------- 1 file changed, 24 insertions(+), 21 deletions(-) diff --git a/.github/workflows/python-format-lint.yml b/.github/workflows/python-format-lint.yml index 61d36e1d0..1448cf408 100644 --- a/.github/workflows/python-format-lint.yml +++ b/.github/workflows/python-format-lint.yml @@ -3,53 +3,56 @@ name: Run Python formatters and linters on: [push] jobs: - black_format: + isort_format: runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 with: - ref: ${{ github.head_ref }} + ref: ${{ github.head_ref }} + + - name: Install isort + run: pip install isort - - name: Check files using the black formatter - uses: rickstaa/action-black@v1 - id: action_black - with: - black_args: "." + - name: Run isort to sort imports + uses: isort/isort-action@v1 - - name: Commit changes if code is formatted + - name: Commit isort changes uses: EndBug/add-and-commit@v9 with: - author_name: Black Robot - author_email: black-robot@example.com - message: 'Committing black-format changes' + author_name: Isort Robot + author_email: isort-robot@example.com + message: 'Committing isort changes' env: GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - isort_format: + black_format: runs-on: ubuntu-latest - needs: black_format + needs: isort_format steps: - uses: actions/checkout@v4 with: ref: ${{ github.head_ref }} - - name: Run isort to sort imports - run: isort . + - name: Check files using the black formatter + uses: rickstaa/action-black@v1 + id: action_black + with: + black_args: "." - - name: Commit isort changes + - name: Commit black-format changes uses: EndBug/add-and-commit@v9 with: - author_name: Isort Robot - author_email: isort-robot@example.com - message: 'Committing isort changes' + author_name: Black Robot + author_email: black-robot@example.com + message: 'Committing black-format changes' env: GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} pylint_check: runs-on: ubuntu-latest - needs: isort_format + needs: black_format steps: - uses: actions/checkout@v4 @@ -58,4 +61,4 @@ jobs: - name: Run pylint run: | - pylint . \ No newline at end of file + pylint . From 975ed609fa1c42617558796bb491ddb23f887104 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 19:01:30 +0200 Subject: [PATCH 26/51] ci: update pylint job and pylint rules --- .github/workflows/python-format-lint.yml | 27 ++++++++++++++---------- .pylintrc | 1 + 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/.github/workflows/python-format-lint.yml b/.github/workflows/python-format-lint.yml index 1448cf408..70aa6f253 100644 --- a/.github/workflows/python-format-lint.yml +++ b/.github/workflows/python-format-lint.yml @@ -10,9 +10,6 @@ jobs: - uses: actions/checkout@v4 with: ref: ${{ github.head_ref }} - - - name: Install isort - run: pip install isort - name: Run isort to sort imports uses: isort/isort-action@v1 @@ -52,13 +49,21 @@ jobs: pylint_check: runs-on: ubuntu-latest - needs: black_format - + needs: black_format + strategy: + matrix: + python-version: ["3.8", "3.9", "3.10"] steps: - - uses: actions/checkout@v4 - with: - ref: ${{ github.head_ref }} + - uses: actions/checkout@v4 + - name: Set up Python ${{ matrix.python-version }} + uses: actions/setup-python@v3 + with: + python-version: ${{ matrix.python-version }} + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install pylint + - name: Analysing the code with pylint + run: | + pylint $(git ls-files '*.py') - - name: Run pylint - run: | - pylint . diff --git a/.pylintrc b/.pylintrc index 973a0293c..0f2d68d5d 100644 --- a/.pylintrc +++ b/.pylintrc @@ -434,6 +434,7 @@ disable=raw-checker-failed, 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 invalid-name, # [C0103] Naming convention not followed (e.g., function, variable, or class names) + 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 From 154d2e6216dacce6c31527f659518348c0f2b033 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 19:05:56 +0200 Subject: [PATCH 27/51] ci: update which python versions pylint test --- .github/workflows/python-format-lint.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/python-format-lint.yml b/.github/workflows/python-format-lint.yml index 70aa6f253..1983184fe 100644 --- a/.github/workflows/python-format-lint.yml +++ b/.github/workflows/python-format-lint.yml @@ -52,7 +52,7 @@ jobs: needs: black_format strategy: matrix: - python-version: ["3.8", "3.9", "3.10"] + python-version: ["3.9", "3.10", "3.11"] steps: - uses: actions/checkout@v4 - name: Set up Python ${{ matrix.python-version }} From fc9ebefd855e52f4e91b47e20f153e0ef7131709 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 19:08:41 +0200 Subject: [PATCH 28/51] ci: update python pipeline to only run on pull_request --- .github/workflows/python-format-lint.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/python-format-lint.yml b/.github/workflows/python-format-lint.yml index 1983184fe..f29de2627 100644 --- a/.github/workflows/python-format-lint.yml +++ b/.github/workflows/python-format-lint.yml @@ -1,6 +1,6 @@ name: Run Python formatters and linters -on: [push] +on: [pull_request] jobs: isort_format: From 002fb32854fb4f6a8aa01099a2c0ffac2aecf864 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 21:42:36 +0200 Subject: [PATCH 29/51] refactor: fix pylint warning C0103 --- .../acoustics_data_record_lib.py | 12 +- .../acoustics_data_record_node.py | 135 +++++------- .../utilities/display_acoustics_data_live.py | 206 +++++++++--------- .../acoustics_interface_lib.py | 36 ++- .../acoustics_interface_node.py | 124 +++-------- auv_setup/launch/orca.launch.py | 8 +- auv_setup/launch/topside.launch.py | 4 +- mission/LCD/sources/IP_lib.py | 8 +- mission/LCD/sources/LCD.py | 62 +++--- mission/LCD/sources/LCD_lib.py | 66 +++--- mission/LCD/sources/power_sense_module_lib.py | 17 +- mission/LCD/sources/pressure_sensor_lib.py | 8 +- .../blackbox/blackbox/blackbox_log_data.py | 44 +--- mission/blackbox/blackbox/blackbox_node.py | 52 ++--- .../power_sense_module_lib.py | 17 +- .../power_sense_module_node.py | 54 ++--- .../pressure_sensor_lib.py | 8 +- .../pressure_sensor_node.py | 36 +-- .../temperature_sensor_node.py | 42 +--- .../joystick_interface_auv.py | 63 +----- .../thruster_interface_auv_driver_lib.py | 103 ++++----- .../thruster_interface_auv_node.py | 58 ++--- 22 files changed, 418 insertions(+), 745 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 d780bee26..c6f3f7847 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 @@ -5,10 +5,10 @@ class AcousticsDataRecordLib: - def __init__(self, ROS2_PACKAGE_DIRECTORY=""): + def __init__(self, ros2_package_directory=""): # Global variables for .csv file manipulation ---------- # Get the path for the directory where we will store our data - self.acoustics_data_directory = ROS2_PACKAGE_DIRECTORY + "acoustics_data/" + self.acoustics_data_directory = ros2_package_directory + "acoustics_data/" timestamp = time.strftime("%Y-%m-%d_%H:%M:%S") data_file_name = "acoustics_data_" + timestamp + ".csv" @@ -29,9 +29,7 @@ def __init__(self, ROS2_PACKAGE_DIRECTORY=""): ] # Make new .csv file for loging 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) @@ -71,9 +69,7 @@ def log_data_to_csv_file( current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] # Write to .csv file - with open( - self.data_file_location, mode="a", newline="", encoding="utf-8" - ) as csv_file: + with open(self.data_file_location, mode="a", newline="", encoding="utf-8") as csv_file: writer = csv.writer(csv_file) writer.writerow( [ diff --git a/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py b/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py index f4c0a3f34..099302e09 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 @@ -16,106 +16,73 @@ class AcousticsDataRecordNode(Node): def __init__(self): # Variables for seting upp loging correctly - hydrophoneDataSize = ( - 2**10 - ) * 3 # 1 hydrophone buffer is 2^10 long, Each hydrophone data has 3 buffers full of this data - DSPDataSize = 2**10 # DSP (Digital Signal Processing) has 2^10 long data - TDOADataSize = ( - 5 # TDOA (Time Difference Of Arrival) has 5 hydrophones it has times for - ) - positionDataSize = 3 # position only has X, Y, Z basicaly 3 elements + 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 + position_data_size = 3 # position only has X, Y, Z basicaly 3 elements # Initialize ROS2 node super().__init__("acoustics_data_record_node") # Initialize Subscribers ---------- # Start listening to Hydrophone data - self.subscriberHydrophone1 = self.create_subscription( - Int32MultiArray, "/acoustics/hydrophone1", self.hydrophone1_callback, 5 - ) - self.hydropone1Data = array.array("i", [0] * hydrophoneDataSize) + self.subscriber_hydrophone1 = self.create_subscription(Int32MultiArray, "/acoustics/hydrophone1", self.hydrophone1_callback, 5) + self.hydropone1_data = array.array("i", [0] * hydrophone_data_size) - self.subscriberHydrophone2 = self.create_subscription( - Int32MultiArray, "/acoustics/hydrophone2", self.hydrophone2_callback, 5 - ) - self.hydropone2Data = array.array("i", [0] * hydrophoneDataSize) + self.subscriber_hydrophone2 = self.create_subscription(Int32MultiArray, "/acoustics/hydrophone2", self.hydrophone2_callback, 5) + self.hydropone2_data = array.array("i", [0] * hydrophone_data_size) - self.subscriberHydrophone3 = self.create_subscription( - Int32MultiArray, "/acoustics/hydrophone3", self.hydrophone3_callback, 5 - ) - self.hydropone3Data = array.array("i", [0] * hydrophoneDataSize) + self.subscriber_hydrophone3 = self.create_subscription(Int32MultiArray, "/acoustics/hydrophone3", self.hydrophone3_callback, 5) + self.hydropone3_data = array.array("i", [0] * hydrophone_data_size) - self.subscriberHydrophone4 = self.create_subscription( - Int32MultiArray, "/acoustics/hydrophone4", self.hydrophone4_callback, 5 - ) - self.hydropone4Data = array.array("i", [0] * hydrophoneDataSize) + self.subscriber_hydrophone4 = self.create_subscription(Int32MultiArray, "/acoustics/hydrophone4", self.hydrophone4_callback, 5) + self.hydropone4_data = array.array("i", [0] * hydrophone_data_size) - self.subscriberHydrophone5 = self.create_subscription( - Int32MultiArray, "/acoustics/hydrophone5", self.hydrophone5_callback, 5 - ) - self.hydropone5Data = array.array("i", [0] * hydrophoneDataSize) + self.subscriber_hydrophone5 = self.create_subscription(Int32MultiArray, "/acoustics/hydrophone5", self.hydrophone5_callback, 5) + self.hydropone5_data = array.array("i", [0] * hydrophone_data_size) # Start listening to DSP (Digital Signal Processing) data - self.subscriberFilterResponse = self.create_subscription( + self.subscriber_filter_response = self.create_subscription( Int32MultiArray, "/acoustics/filter_response", self.filter_response_callback, 5, ) - self.filterResponseData = array.array("i", [0] * DSPDataSize) + self.filter_response_data = array.array("i", [0] * dsp_data_size) - self.subscriberFFT = self.create_subscription( - Int32MultiArray, "/acoustics/fft", self.fft_callback, 5 - ) - self.FFTData = array.array("i", [0] * DSPDataSize) + self.subscriber_fft = self.create_subscription(Int32MultiArray, "/acoustics/fft", self.fft_callback, 5) + self.fft_data = array.array("i", [0] * dsp_data_size) - self.subscriberPeaks = self.create_subscription( - Int32MultiArray, "/acoustics/peaks", self.peaks_callback, 5 - ) - self.peaksData = array.array("i", [0] * DSPDataSize) + 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 - self.subscriberTDOAResponse = self.create_subscription( + self.subscriber_tdoa_response = self.create_subscription( Float32MultiArray, "/acoustics/time_difference_of_arrival", self.tdoa_callback, 5, ) - self.TDOAData = array.array("f", [0.0] * TDOADataSize) + self.tdoa_data = array.array("f", [0.0] * tdoa_data_size) - self.subscriberPositionResponse = self.create_subscription( - Float32MultiArray, "/acoustics/position", self.position_callback, 5 - ) - self.positionData = array.array("f", [0.0] * positionDataSize) + 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 = 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 + "/../../../../" - ) # 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 loging 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, verry slow - DATA_LOGING_RATE = ( - self.get_parameter("acoustics.data_logging_rate") - .get_parameter_value() - .double_value - ) - timer_period = 1.0 / DATA_LOGING_RATE + self.declare_parameter("acoustics.data_logging_rate", 1.0) # Providing a default value 1.0 => 1 samplings per second, verry 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) # Debuging ---------- @@ -141,7 +108,7 @@ def hydrophone1_callback(self, msg): Args: msg (Int32MultiArray): Message containing hydrophone1 data. """ - self.hydropone1Data = msg.data + self.hydropone1_data = msg.data def hydrophone2_callback(self, msg): """ @@ -150,7 +117,7 @@ def hydrophone2_callback(self, msg): Args: msg (Int32MultiArray): Message containing hydrophone2 data. """ - self.hydropone2Data = msg.data + self.hydropone2_data = msg.data def hydrophone3_callback(self, msg): """ @@ -159,7 +126,7 @@ def hydrophone3_callback(self, msg): Args: msg (Int32MultiArray): Message containing hydrophone3 data. """ - self.hydropone3Data = msg.data + self.hydropone3_data = msg.data def hydrophone4_callback(self, msg): """ @@ -168,7 +135,7 @@ def hydrophone4_callback(self, msg): Args: msg (Int32MultiArray): Message containing hydrophone4 data. """ - self.hydropone4Data = msg.data + self.hydropone4_data = msg.data def hydrophone5_callback(self, msg): """ @@ -177,7 +144,7 @@ def hydrophone5_callback(self, msg): Args: msg (Int32MultiArray): Message containing hydrophone5 data. """ - self.hydropone5Data = msg.data + self.hydropone5_data = msg.data def filter_response_callback(self, msg): """ @@ -186,7 +153,7 @@ def filter_response_callback(self, msg): Args: msg (Int32MultiArray): Message containing filter response data. """ - self.filterResponseData = msg.data + self.filter_response_data = msg.data def fft_callback(self, msg): """ @@ -195,7 +162,7 @@ def fft_callback(self, msg): Args: msg (Int32MultiArray): Message containing FFT data. """ - self.FFTData = msg.data + self.fft_data = msg.data def peaks_callback(self, msg): """ @@ -204,7 +171,7 @@ def peaks_callback(self, msg): Args: msg (Int32MultiArray): Message containing peaks data. """ - self.peaksData = msg.data + self.peaks_data = msg.data def tdoa_callback(self, msg): """ @@ -213,7 +180,7 @@ def tdoa_callback(self, msg): Args: msg (Float32MultiArray): Message containing TDOA data. """ - self.TDOAData = msg.data + self.tdoa_data = msg.data def position_callback(self, msg): """ @@ -222,7 +189,7 @@ def position_callback(self, msg): Args: msg (Float32MultiArray): Message containing position data. """ - self.positionData = msg.data + self.position_data = msg.data # The logger that logs all the data def logger(self): @@ -232,16 +199,16 @@ def logger(self): This method is called periodically based on the data logging rate. """ self.acoustics_data_record.log_data_to_csv_file( - hydrophone1=self.hydropone1Data, - hydrophone2=self.hydropone2Data, - hydrophone3=self.hydropone3Data, - hydrophone4=self.hydropone4Data, - hydrophone5=self.hydropone5Data, - filter_response=self.filterResponseData, - fft=self.FFTData, - peaks=self.peaksData, - tdoa=self.TDOAData, - position=self.positionData, + hydrophone1=self.hydropone1_data, + hydrophone2=self.hydropone2_data, + hydrophone3=self.hydropone3_data, + hydrophone4=self.hydropone4_data, + hydrophone5=self.hydropone5_data, + filter_response=self.filter_response_data, + fft=self.fft_data, + peaks=self.peaks_data, + tdoa=self.tdoa_data, + position=self.position_data, ) 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 39488581b..1d36ca7c4 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -13,12 +13,10 @@ from matplotlib import animation, gridspec # Variables for seting upp data structures correctly -hydrophoneDataSize = ( - 2**10 -) * 3 # 1 hydrophone buffer is 2^10 long, Each hydrophone data has 3 buffers full of this data -DSPDataSize = 2**10 # DSP (Digital Signal Processing) has 2^10 long data -TDOADataSize = 5 # TDOA (Time Difference Of Arrival) has 5 hydrophones it has times for -positionDataSize = 3 # position only has X, Y, Z basicaly 3 elements +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 +POSITION_DATA_SIZE = 3 # position only has X, Y, Z basicaly 3 elements # Important variables for later processing of data SAMPLE_RATE = 430_000 # 430 kHz @@ -30,21 +28,15 @@ # 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") @@ -66,15 +58,13 @@ 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 convertPandasObjectToIntArray(pandasObject): +def convert_pandas_object_to_int_array(pandas_object): """ Convert a pandas object containing a string representation of an integer array to a list of integers. @@ -84,13 +74,13 @@ def convertPandasObjectToIntArray(pandasObject): Returns: list: A list of integers extracted from the pandas object. """ - pandasString = pandasObject.iloc[0].strip("array('i', ").rstrip(")") - pandasIntArray = [int(x.strip()) for x in pandasString.strip("[]").split(",")] + pandas_string = pandas_object.iloc[0].strip("array('i', ").rstrip(")") + pandas_int_array = [int(x.strip()) for x in pandas_string.strip("[]").split(",")] - return pandasIntArray + return pandas_int_array -def convertPandasObjectToFloatArray(pandasObject): +def convert_pandas_object_to_float_array(pandas_object): """ Convert a pandas object containing a string representation of a float array to a list of floats. @@ -100,13 +90,13 @@ def convertPandasObjectToFloatArray(pandasObject): Returns: list: A list of floats extracted from the pandas object. """ - pandasString = pandasObject.iloc[0].strip("array('f', ").rstrip(")") - pandasFloatArray = [float(x.strip()) for x in pandasString.strip("[]").split(",")] + pandas_string = pandas_object.iloc[0].strip("array('f', ").rstrip(")") + pandas_float_array = [float(x.strip()) for x in pandas_string.strip("[]").split(",")] - return pandasFloatArray + return pandas_float_array -def getAcousticsData(): +def get_acoustics_data(): """ Retrieves and processes the latest acoustics data from a CSV file. @@ -134,53 +124,53 @@ def getAcousticsData(): 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] * hydrophoneDataSize - hydrophone2 = [0] * hydrophoneDataSize - hydrophone3 = [0] * hydrophoneDataSize - hydrophone4 = [0] * hydrophoneDataSize - hydrophone5 = [0] * hydrophoneDataSize - - unfilteredData = [0] * DSPDataSize - filteredData = [0] * DSPDataSize - FFTData = [0] * DSPDataSize - peaksData = [0] * DSPDataSize - FFTAmplitudeData = [0] * DSPDataSize - FFTFrequencyData = [0] * DSPDataSize - peaksAmplitudeData = [0] * DSPDataSize - peaksFrequencyData = [0] * DSPDataSize - - tdoaData = [0.0] * TDOADataSize - positonData = [0.0] * positionDataSize + hydrophone1 = [0] * HYDROPHONE_DATA_SIZE + hydrophone2 = [0] * HYDROPHONE_DATA_SIZE + hydrophone3 = [0] * HYDROPHONE_DATA_SIZE + hydrophone4 = [0] * HYDROPHONE_DATA_SIZE + hydrophone5 = [0] * HYDROPHONE_DATA_SIZE + + unfiltered_data = [0] * DSP_DATA_SIZE + filtered_data = [0] * DSP_DATA_SIZE + fft_data = [0] * DSP_DATA_SIZE + peaks_data = [0] * DSP_DATA_SIZE + fft_amplitude_data = [0] * DSP_DATA_SIZE + fft_frequency_data = [0] * DSP_DATA_SIZE + peaks_amplitude_data = [0] * DSP_DATA_SIZE + peaks_frequency_data = [0] * DSP_DATA_SIZE + + tdoa_data = [0.0] * TDOA_DATA_SIZE + positon_data = [0.0] * POSITION_DATA_SIZE # Read latest acoustics data ---------- - acousticsDataFrame = pd.read_csv(acousticsCSVFile) - latestAcousticsData = acousticsDataFrame.tail(1) + acoustics_data_frame = pd.read_csv(acousticsCSVFile) + latest_acoustics_data = acoustics_data_frame.tail(1) try: # Get latest hydrophone data - hydrophone1 = convertPandasObjectToIntArray(latestAcousticsData["Hydrophone1"]) - hydrophone2 = convertPandasObjectToIntArray(latestAcousticsData["Hydrophone2"]) - hydrophone3 = convertPandasObjectToIntArray(latestAcousticsData["Hydrophone3"]) - hydrophone4 = convertPandasObjectToIntArray(latestAcousticsData["Hydrophone4"]) - hydrophone5 = convertPandasObjectToIntArray(latestAcousticsData["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 # Hydrohones have a ring buffer the size of 3 buffers each containing 1024 values (2^10) # We always use the first ring buffer of Hydrophone 1 to performe DSP # That is why unfiltered data is the same as Hydrphne 1 first buffer - unfilteredData = hydrophone1[0:1024] + unfiltered_data = hydrophone1[0:1024] # Get DSP data - filteredData = convertPandasObjectToIntArray( - latestAcousticsData["FilterResponse"] + filtered_data = convert_pandas_object_to_int_array( + latest_acoustics_data["FilterResponse"] ) # Also known as Filter response to the raw unfiltered data - FFTData = convertPandasObjectToIntArray(latestAcousticsData["FFT"]) - peaksData = convertPandasObjectToIntArray(latestAcousticsData["Peaks"]) + fft_data = convert_pandas_object_to_int_array(latest_acoustics_data["FFT"]) + peaks_data = convert_pandas_object_to_int_array(latest_acoustics_data["Peaks"]) # Get multilateration data - tdoaData = convertPandasObjectToFloatArray(latestAcousticsData["TDOA"]) - positonData = convertPandasObjectToFloatArray(latestAcousticsData["Position"]) + tdoa_data = convert_pandas_object_to_float_array(latest_acoustics_data["TDOA"]) + 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}") @@ -188,25 +178,25 @@ def getAcousticsData(): # 1. Convert FFTData to its corresponding frequency amount # 2. Cut out big FFT frequencies out as they are not relevant # 3. Cut out big peak frequencies as they are not relevant - sampleLength = len(FFTData) - maxFrequencyIndex = int(MAX_FREQUENCY_TO_SHOW * sampleLength / SAMPLE_RATE) + sample_length = len(fft_data) + max_frequency_index = int(MAX_FREQUENCY_TO_SHOW * sample_length / SAMPLE_RATE) - FFTAmplitudeData = FFTData[0:maxFrequencyIndex] - FFTFrequencyData = [(i * (SAMPLE_RATE / sampleLength)) for i in range(sampleLength)] - FFTFrequencyData = FFTFrequencyData[0:maxFrequencyIndex] + 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 = 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 # We want to get amplitude and frequency, dont really care about the phase try: - tempAmplitude = [] - tempFrequency = [] - for peak_index in range(1, len(peaksData), 3): - if peaksData[peak_index] < MAX_FREQUENCY_TO_SHOW: - tempAmplitude += [peaksData[peak_index - 1]] - tempFrequency += [peaksData[peak_index]] - - peaksAmplitudeData = tempAmplitude - peaksFrequencyData = tempFrequency + temp_amplitude = [] + temp_frequency = [] + for peak_index in range(1, len(peaks_data), 3): + if peaks_data[peak_index] < MAX_FREQUENCY_TO_SHOW: + temp_amplitude += [peaks_data[peak_index - 1]] + temp_frequency += [peaks_data[peak_index]] + + peaks_amplitude_data = temp_amplitude + peaks_frequency_data = temp_frequency except Exception as e: print(f"ERROR processing DSP data. Exception: {e}") @@ -217,14 +207,14 @@ def getAcousticsData(): hydrophone3, hydrophone4, hydrophone5, - unfilteredData, - filteredData, - FFTAmplitudeData, - FFTFrequencyData, - peaksAmplitudeData, - peaksFrequencyData, - tdoaData, - positonData, + unfiltered_data, + filtered_data, + fft_amplitude_data, + fft_frequency_data, + peaks_amplitude_data, + peaks_frequency_data, + tdoa_data, + positon_data, ] @@ -253,35 +243,35 @@ def display_live_data(): `colorSoftBlue`, `colorSoftGreen`, and `colorSoftPurple` are defined elsewhere in the code. """ # Get latest acoustics data - acousticsData = getAcousticsData() + acoustics_data = get_acoustics_data() # Set the lates acoustics data in apropriate variables - hydrophoneData = [ - acousticsData[0], # Hydrophone 1 - acousticsData[1], # Hydrophone 2 - acousticsData[2], # Hydrophone 3 - acousticsData[3], # Hydrophone 4 - acousticsData[4], # Hydrophone 5 + hydrophone_data = [ + acoustics_data[0], # Hydrophone 1 + acoustics_data[1], # Hydrophone 2 + acoustics_data[2], # Hydrophone 3 + acoustics_data[3], # Hydrophone 4 + acoustics_data[4], # Hydrophone 5 ] - unfilteredData = acousticsData[5] + unfiltered_data = acoustics_data[5] - filterData = acousticsData[6] - FFTAmplitudeData = acousticsData[7] - FFTFrequencyData = acousticsData[8] - peaksAmplitudeData = acousticsData[9] - peaksFrequencyData = acousticsData[10] + filter_data = acoustics_data[6] + fft_amplitude_data = acoustics_data[7] + fft_frequency_data = acoustics_data[8] + peaks_amplitude_data = acoustics_data[9] + peaks_frequency_data = acoustics_data[10] - tdoaData = acousticsData[11] # Currently not in use - positionData = acousticsData[12] # Currently not in use + tdoa_data = acoustics_data[11] # Currently not in use + position_data = acoustics_data[12] # Currently not in use # Plot hydrophone data for hydrophone_index in range(5): - xHydrophone = list(range(len(hydrophoneData[hydrophone_index][::]))) + x_hydrophone = list(range(len(hydrophone_data[hydrophone_index][::]))) hydrophoneAxis[hydrophone_index].clear() hydrophoneAxis[hydrophone_index].plot( - xHydrophone, - hydrophoneData[hydrophone_index], + x_hydrophone, + hydrophone_data[hydrophone_index], label=f"Hydrophone {hydrophone_index + 1}", color=colorSoftBlue, alpha=1, @@ -289,14 +279,12 @@ def display_live_data(): hydrophoneAxis[hydrophone_index].legend(loc="upper right", fontsize="xx-small") # Plot Filter response - xRaw = list(range(len(unfilteredData))) - xFilter = list(range(len(filterData))) + x_raw = list(range(len(unfiltered_data))) + x_filter = list(range(len(filter_data))) filterAxis.clear() filterAxis.set_title("Filter response") - filterAxis.plot(xRaw, unfilteredData, label="Raw", color=colorSoftBlue, alpha=0.5) - filterAxis.plot( - xFilter, filterData, label="Filter", color=colorSoftGreen, alpha=0.7 - ) + filterAxis.plot(x_raw, unfiltered_data, label="Raw", color=colorSoftBlue, alpha=0.5) + filterAxis.plot(x_filter, filter_data, label="Filter", color=colorSoftGreen, alpha=0.7) filterAxis.legend(loc="upper right", fontsize="xx-small") # Plot FFT data @@ -305,16 +293,16 @@ def display_live_data(): FFTAxis.set_xlabel("Frequency [Hz]") FFTAxis.set_ylabel("Amplitude") FFTAxis.bar( - FFTFrequencyData, - FFTAmplitudeData, + fft_frequency_data, + fft_amplitude_data, label="FFT", color=colorSoftPurple, alpha=1, width=500, ) FFTAxis.scatter( - peaksFrequencyData, - peaksAmplitudeData, + peaks_frequency_data, + peaks_amplitude_data, label="Peaks", color="red", alpha=0.7, @@ -325,7 +313,7 @@ def display_live_data(): FFTAxis.legend(loc="upper right", fontsize="xx-small") # Print out the unused Multilateration data - print(f"TDOA Data: {tdoaData} | Position Data: {positionData}") + print(f"TDOA Data: {tdoa_data} | Position Data: {position_data}") # Plotting live data diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index a24f812f4..2bb51dc4d 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -74,18 +74,16 @@ class TeensyCommunicationUDP: } @classmethod - def init_communication(cls, frequenciesOfInterest: list[tuple[int, int]]) -> None: + 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(frequenciesOfInterest) == 10 - ), "Frequency list has to have exactly 10 entries" + assert len(frequencies_of_interest) == 10, "Frequency list has to have exactly 10 entries" - _frequenciesOfInterest = frequenciesOfInterest + _frequencies_of_interest = frequencies_of_interest cls.MY_IP = cls._get_ip() @@ -95,21 +93,21 @@ def init_communication(cls, frequenciesOfInterest: list[tuple[int, int]]) -> Non cls._clientSocket.setblocking(False) cls._send_acknowledge_signal() - timeStart = time.time() + time_start = time.time() # Wait for READY signal while not cls._check_if_available(): print("Did not receive READY signal. Will wait.") time.sleep(1) - if time.time() - timeStart > cls._timeoutMax: + if time.time() - time_start > cls._timeoutMax: print("Gave up on receiving READY. Sending acknowledge signal again") # Start over - timeStart = time.time() + time_start = time.time() cls._send_acknowledge_signal() print("READY signal received, sending frequencies...") - cls._send_frequencies_of_interest(frequenciesOfInterest) + cls._send_frequencies_of_interest(frequencies_of_interest) @classmethod def fetch_data(cls) -> None: @@ -166,8 +164,8 @@ def _get_raw_data(cls) -> str | None: """ try: rec_data, _ = cls._clientSocket.recvfrom(cls._MAX_PACKAGE_SIZE_RECEIVED) - messageReceived = rec_data.decode() - return messageReceived + message_received = rec_data.decode() + return message_received except error as e: # `error` is really `socket.error` if e.errno == errno.EWOULDBLOCK: pass @@ -209,13 +207,13 @@ def _get_ip(cls) -> None: try: # doesn't even have to be reachable s.connect((cls._TEENSY_IP, 1)) - IP = s.getsockname()[0] + ip = s.getsockname()[0] except Exception: - IP = "127.0.0.1" + ip = "127.0.0.1" finally: s.close() - return IP + return ip @classmethod def _send_acknowledge_signal(cls) -> None: @@ -261,9 +259,7 @@ def _check_if_available(cls) -> None: return False @classmethod - def _send_frequencies_of_interest( - cls, frequenciesOfInterest: list[tuple[float, float]] - ) -> None: + def _send_frequencies_of_interest(cls, frequencies_of_interest: list[tuple[float, float]]) -> None: """ Sends the list of frequencies with variance to teensy @@ -272,12 +268,10 @@ def _send_frequencies_of_interest( """ try: # Format (CSV): xxx,x,xx,x...,x (frequency list comes first, then variances) - assert ( - len(frequenciesOfInterest) == 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 frequenciesOfInterest: + for frequency, variance in frequencies_of_interest: frequency_variance_msg = f"{str(frequency)},{str(variance)}," # print(self.address); diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py index bf668cbcc..a4b1d94ab 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py @@ -25,70 +25,38 @@ def __init__(self) -> None: 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, verry slow - DATA_LOGING_RATE = ( - self.get_parameter("acoustics.data_logging_rate") - .get_parameter_value() - .double_value - ) - timer_period = 1.0 / DATA_LOGING_RATE + self.declare_parameter("acoustics.data_logging_rate", 1.0) # Providing a default value 1.0 => 1 samplings per second, verry 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 - ) - - frequenciesOfInterest = [] - for i in range(0, len(FREQUENCIES_OF_INTEREST_PARAMETERS), 2): - frequenciesOfInterest += [ + 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): + frequencies_of_interest += [ ( - FREQUENCIES_OF_INTEREST_PARAMETERS[i], - FREQUENCIES_OF_INTEREST_PARAMETERS[i + 1], + frequencies_of_interest_parameters[i], + frequencies_of_interest_parameters[i + 1], ) ] @@ -97,7 +65,7 @@ def __init__(self) -> None: self.get_logger().info("Acoustics PCB MCU IP: 10.0.0.111") self.get_logger().info("Trying to connect...") - TeensyCommunicationUDP.init_communication(frequenciesOfInterest) + TeensyCommunicationUDP.init_communication(frequencies_of_interest) self.get_logger().info("Sucsefully connected to Acoustics PCB MCU :D") @@ -112,40 +80,18 @@ 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=None): diff --git a/auv_setup/launch/orca.launch.py b/auv_setup/launch/orca.launch.py index 1d74c83fc..34a88f02f 100755 --- a/auv_setup/launch/orca.launch.py +++ b/auv_setup/launch/orca.launch.py @@ -20,9 +20,7 @@ def generate_launch_description(): 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( @@ -47,6 +45,4 @@ def generate_launch_description(): ) # 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 63e2c4fe4..4058b79cf 100755 --- a/auv_setup/launch/topside.launch.py +++ b/auv_setup/launch/topside.launch.py @@ -20,9 +20,7 @@ def generate_launch_description(): 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 2d5563d1f..c16ea0538 100644 --- a/mission/LCD/sources/IP_lib.py +++ b/mission/LCD/sources/IP_lib.py @@ -7,14 +7,14 @@ class IPDriver: def __init__(self): self.cmd = "hostname -I | cut -d' ' -f1" - def get_IP(self): + def get_ip(self): """ Executes a shell command to retrieve the IP address. Returns: str: The IP address as a string. """ - IP_bytes = subprocess.check_output(self.cmd, shell=True) - IP_str = IP_bytes.decode("utf-8") + ip_bytes = subprocess.check_output(self.cmd, shell=True) + ip_str = ip_bytes.decode("utf-8") - return IP_str + return ip_str diff --git a/mission/LCD/sources/LCD.py b/mission/LCD/sources/LCD.py index 1946e1afd..8d4ed0356 100644 --- a/mission/LCD/sources/LCD.py +++ b/mission/LCD/sources/LCD.py @@ -30,20 +30,18 @@ def format_line(value: str, unit: str): Returns: str: A formatted string that fits within a 16-character limit, with the unit appended. """ - spacesAvailable = 16 - valueLenght = len(value) - unitLenght = ( - len(unit) + 1 - ) # +1 to make sure there is spacing between value and unit + spaces_available = 16 + value_length = len(value) + unit_length = len(unit) + 1 # +1 to make sure there is spacing between value and unit - emptySpaceLenght = spacesAvailable - (valueLenght + unitLenght) - emptySpaceLenght = max(emptySpaceLenght, 0) + empty_space_length = spaces_available - (value_length + unit_length) + empty_space_length = max(empty_space_length, 0) - formatedString = value[0 : (spacesAvailable - unitLenght)] - formatedString += " " * emptySpaceLenght - formatedString += " " + unit + formated_string = value[0 : (spaces_available - unit_length)] + formated_string += " " * empty_space_length + formated_string += " " + unit - return formatedString + return formated_string # Fancy animation at the start @@ -52,28 +50,28 @@ def format_line(value: str, unit: str): # Display information on the LDC Screen while True: # IP ---------- - timeDispaying = 5 - updatesPerSecond = 1 - for i in range(timeDispaying * updatesPerSecond): - line1 = "IP: " - line2 = str(IP.get_IP()) - LCD.write_to_screen(line1, line2) - sleep(1 / updatesPerSecond) + TIME_DISPLAYING = 5 + UPDATES_PER_SECOND = 1 + 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) + sleep(1 / UPDATES_PER_SECOND) # Voltage and Current ---------- - timeDispaying = 5 - updatesPerSecond = 2 - for i in range(timeDispaying * updatesPerSecond): - line1 = format_line(str(round(PSM.get_voltage(), 3)), "V") - line2 = format_line(str(round(PSM.get_current(), 3)), "A") - LCD.write_to_screen(line1, line2) - sleep(1 / updatesPerSecond) + TIME_DISPLAYING = 5 + UPDATES_PER_SECOND = 2 + 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) + sleep(1 / UPDATES_PER_SECOND) # Pressure and Temperature ---------- - timeDispaying = 5 - updatesPerSecond = 1 - for i in range(timeDispaying * updatesPerSecond): - line1 = format_line(str(round(Pressure.get_pressure(), 1)), "hPa") - line2 = format_line(str(round(Temperature.get_temperature(), 1)), "*C") - LCD.write_to_screen(line1, line2) - sleep(1 / updatesPerSecond) + TIME_DISPLAYING = 5 + UPDATES_PER_SECOND = 1 + 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) + sleep(1 / UPDATES_PER_SECOND) diff --git a/mission/LCD/sources/LCD_lib.py b/mission/LCD/sources/LCD_lib.py index 20148e5ef..bce20eb75 100644 --- a/mission/LCD/sources/LCD_lib.py +++ b/mission/LCD/sources/LCD_lib.py @@ -11,7 +11,7 @@ def __init__(self): # Initialize LCD Screen lcd_i2c_address = 0x27 - self._LCD = CharLCD( + self._lcd = CharLCD( i2c_expander="PCF8574", address=lcd_i2c_address, port=1, @@ -37,15 +37,15 @@ def write_to_screen(self, line1: str = "", line2: str = ""): line2 (str): The text to display on the second line of the LCD screen. Defaults to an empty string. """ - self._LCD.clear() + self._lcd.clear() # limit line size as to big of a line destroys the view - spacesAvailable = 16 - line1 = line1[0:spacesAvailable] - line2 = line2[0:spacesAvailable] + spaces_available = 16 + line1 = line1[0:spaces_available] + line2 = line2[0:spaces_available] - self._LCD.write_string(line1 + "\r\n") - self._LCD.write_string(line2) + self._lcd.write_string(line1 + "\r\n") + self._lcd.write_string(line2) def fancy_animation(self, animation_speed=0.4): """ @@ -102,66 +102,58 @@ def fancy_animation(self, animation_speed=0.4): # Ghost chaisng Packman ---------- # Load the custom characters into the LCD - self._LCD.create_char(0, char_pacman_left_open[0]) - self._LCD.create_char(1, char_pacman_closed[0]) - self._LCD.create_char(2, char_ghost[0]) + self._lcd.create_char(0, char_pacman_left_open[0]) + self._lcd.create_char(1, char_pacman_closed[0]) + self._lcd.create_char(2, char_ghost[0]) # Display sequence steps = 20 - for a in range( - steps - ): # Increase range to allow characters to exit screen completely - self._LCD.clear() + for a in range(steps): # Increase range to allow characters to exit screen completely + self._lcd.clear() # Pac-Man position and animation if a < 16: # Continue displaying Pac-Man until he's off-screen pac_man_pos = (0, a) - self._LCD.cursor_pos = pac_man_pos + self._lcd.cursor_pos = pac_man_pos if a % 2 == 0: - self._LCD.write_string(chr(0)) # Mouth open + self._lcd.write_string(chr(0)) # Mouth open else: - self._LCD.write_string(chr(1)) # Mouth closed + 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)) + self._lcd.cursor_pos = ghost_pos + self._lcd.write_string(chr(2)) sleep(animation_speed) # Packman Chasing Ghost ---------- # Load the custom characters into the LCD - self._LCD.create_char(0, char_pacman_right_open[0]) - self._LCD.create_char(1, char_pacman_closed[0]) - self._LCD.create_char(2, char_ghost[0]) + self._lcd.create_char(0, char_pacman_right_open[0]) + self._lcd.create_char(1, char_pacman_closed[0]) + self._lcd.create_char(2, char_ghost[0]) # Display sequence steps = 26 - for a in range( - steps + 4 - ): # Adjusted range to ensure all characters exit screen - self._LCD.clear() + for a in range(steps + 4): # Adjusted range to ensure all characters exit screen + self._lcd.clear() # Ghost position and animation ghost_start_pos = steps - 1 # Adjusted for initial off-screen to the right ghost_current_pos = ghost_start_pos - a if 0 <= ghost_current_pos < 16: - self._LCD.cursor_pos = (1, ghost_current_pos) - self._LCD.write_string(chr(2)) + self._lcd.cursor_pos = (1, ghost_current_pos) + 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) + self._lcd.cursor_pos = (1, pac_man_current_pos) if a % 2 == 0: - self._LCD.write_string(chr(0)) # Mouth open + self._lcd.write_string(chr(0)) # Mouth open else: - self._LCD.write_string(chr(1)) # Mouth closed + self._lcd.write_string(chr(1)) # Mouth closed sleep(animation_speed * 0.3) diff --git a/mission/LCD/sources/power_sense_module_lib.py b/mission/LCD/sources/power_sense_module_lib.py index e42efb969..477fd60bc 100755 --- a/mission/LCD/sources/power_sense_module_lib.py +++ b/mission/LCD/sources/power_sense_module_lib.py @@ -23,12 +23,8 @@ def __init__(self): 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}") @@ -51,9 +47,7 @@ def get_voltage(self): """ # 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}") @@ -71,10 +65,7 @@ def get_current(self): 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 55e990ea8..fbab595a7 100755 --- a/mission/LCD/sources/pressure_sensor_lib.py +++ b/mission/LCD/sources/pressure_sensor_lib.py @@ -11,14 +11,14 @@ class PressureSensor: def __init__(self): # Pressure Sensor Setup - i2c_adress_MPRLS = 0x18 # Reads pressure from MPRLS Adafruit sensor + i2c_adress_mprls = 0x18 # Reads pressure from MPRLS Adafruit sensor self.channel_pressure = None try: - I2CBuss = board.I2C() + i2cbus = board.I2C() self.channel_pressure = adafruit_mprls.MPRLS( - i2c_bus=I2CBuss, - addr=i2c_adress_MPRLS, + i2c_bus=i2cbus, + addr=i2c_adress_mprls, reset_pin=None, eoc_pin=None, psi_min=0, diff --git a/mission/blackbox/blackbox/blackbox_log_data.py b/mission/blackbox/blackbox/blackbox_log_data.py index 0cdbd9b10..6642d46ad 100755 --- a/mission/blackbox/blackbox/blackbox_log_data.py +++ b/mission/blackbox/blackbox/blackbox_log_data.py @@ -9,10 +9,10 @@ class BlackBoxLogData: - def __init__(self, ROS2_PACKAGE_DIRECTORY=""): + def __init__(self, ros2_package_directory=""): # Global variables for .csv file manipulation ---------- # Get the path for the directory where we will store our data - self.blackbox_data_directory = ROS2_PACKAGE_DIRECTORY + "blackbox_data/" + self.blackbox_data_directory = ros2_package_directory + "blackbox_data/" timestamp = time.strftime("%Y-%m-%d_%H:%M:%S") data_file_name = "blackbox_data_" + timestamp + ".csv" @@ -48,9 +48,7 @@ def __init__(self, ROS2_PACKAGE_DIRECTORY=""): self.manage_csv_files() # Make new .csv file for loging 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) @@ -79,16 +77,10 @@ def manage_csv_files(self, max_file_age_in_days=7, max_size_kb=3_000_000): 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) @@ -100,9 +92,7 @@ def manage_csv_files(self, max_file_age_in_days=7, max_size_kb=3_000_000): 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: @@ -113,28 +103,20 @@ def manage_csv_files(self, max_file_age_in_days=7, max_size_kb=3_000_000): # 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: @@ -155,9 +137,7 @@ def manage_csv_files(self, max_file_age_in_days=7, max_size_kb=3_000_000): ) / 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 ---------- @@ -214,9 +194,7 @@ def log_data_to_csv_file( current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] # Write to .csv file - with open( - self.data_file_location, mode="a", newline="", encoding="utf-8" - ) as csv_file: + with open(self.data_file_location, mode="a", newline="", encoding="utf-8") as csv_file: writer = csv.writer(csv_file) writer.writerow( [ diff --git a/mission/blackbox/blackbox/blackbox_node.py b/mission/blackbox/blackbox/blackbox_node.py index 921639216..fd4e6737e 100755 --- a/mission/blackbox/blackbox/blackbox_node.py +++ b/mission/blackbox/blackbox/blackbox_node.py @@ -21,63 +21,37 @@ def __init__(self): 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 loging 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, verry slow - DATA_LOGING_RATE = ( - self.get_parameter("blackbox.data_logging_rate") - .get_parameter_value() - .double_value - ) - timer_period = 1.0 / DATA_LOGING_RATE + self.declare_parameter("blackbox.data_logging_rate", 1.0) # Providing a default value 1.0 => 1 samplings per second, verry 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) # Debuging ---------- 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 505510938..596f95ead 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,12 +23,8 @@ def __init__(self): 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}") @@ -52,9 +48,7 @@ def get_voltage(self): """ # 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}") @@ -74,10 +68,7 @@ def get_current(self): 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 bdfae7275..920bf13ad 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 @@ -12,15 +12,11 @@ class PowerSenseModulePublisher(Node): def __init__(self): # Node setup ---------- super().__init__("power_sense_module_publisher") - self.PSM = internal_status_auv.power_sense_module_lib.PowerSenseModule() + 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 @@ -29,11 +25,7 @@ def __init__(self): 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) @@ -41,29 +33,15 @@ def __init__(self): self.logger = get_logger("power_sense_module") self.declare_parameter("internal_status.voltage_min", 14.5) - self.voltageMin = ( - 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.voltageMax = ( - 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) # Debuging ---------- self.get_logger().info('"power_sense_module_publisher" has been started') @@ -76,8 +54,8 @@ def read_timer_callback(self): and publishes it to the corresponding ROS2 topics. """ # Get the PSM data - self.current = self.PSM.get_current() - self.voltage = self.PSM.get_voltage() + self.current = self.psm.get_current() + self.voltage = self.psm.get_voltage() # Publish PSM data current_msg = Float32() @@ -86,12 +64,8 @@ def read_timer_callback(self): 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): """ @@ -100,9 +74,9 @@ def warning_timer_callback(self): 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. """ - if self.voltage < self.voltageMin: + if self.voltage < self.voltage_min: self.logger.fatal(f"WARNING: Battery Voltage to LOW at {self.voltage} V") - elif self.voltage > self.voltageMax: + elif self.voltage > self.voltage_max: self.logger.fatal(f"WARNING: Battery Voltage to HIGH at {self.voltage} V") 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 53114d3e1..6b209b655 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 @@ -11,14 +11,14 @@ class PressureSensor: def __init__(self): # Pressure Sensor Setup - i2c_adress_MPRLS = 0x18 # Reads pressure from MPRLS Adafruit sensor + i2c_adress_mprls = 0x18 # Reads pressure from MPRLS Adafruit sensor self.channel_pressure = None try: - I2CBuss = board.I2C() + i2c_bus = board.I2C() self.channel_pressure = adafruit_mprls.MPRLS( - i2c_bus=I2CBuss, - addr=i2c_adress_MPRLS, + i2c_bus=i2c_bus, + addr=i2c_adress_mprls, reset_pin=None, eoc_pin=None, psi_min=0, 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 24cc718bf..5a8c7b84f 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 @@ -11,7 +11,7 @@ class PressurePublisher(Node): def __init__(self): # Pressure sensor setup ---------- - self.Pressure = internal_status_auv.pressure_sensor_lib.PressureSensor() + self.pressure = internal_status_auv.pressure_sensor_lib.PressureSensor() # Node setup ---------- super().__init__("pressure_sensor_publisher") @@ -22,14 +22,8 @@ def __init__(self): # 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) @@ -37,22 +31,12 @@ def __init__(self): self.logger = get_logger("pressure_sensor") self.declare_parameter("internal_status.pressure_critical_level", 1000.0) - self.pressureCriticalLevel = ( - 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) # Debuging ---------- self.get_logger().info('"pressure_sensor_publisher" has been started') @@ -65,7 +49,7 @@ def timer_callback(self): and publishes it to the "/auv/pressure" topic. """ # Get pressure data - self.pressure = self.Pressure.get_pressure() + self.pressure = self.pressure.get_pressure() # Publish pressure data pressure_msg = Float32() @@ -79,10 +63,8 @@ def warning_timer_callback(self): 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.pressureCriticalLevel: - self.logger.fatal( - f"WARNING: Internal pressure to HIGH: {self.pressure} hPa! Drone might be LEAKING!" - ) + if self.pressure > self.pressure_critical_level: + self.logger.fatal(f"WARNING: Internal pressure to HIGH: {self.pressure} hPa! Drone might be LEAKING!") def main(args=None): 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 f8d25d45b..45a862fc1 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 @@ -11,29 +11,19 @@ class TemperaturePublisher(Node): def __init__(self): # 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) @@ -41,22 +31,12 @@ def __init__(self): self.logger = get_logger("temperature_sensor") self.declare_parameter("internal_status.temperature_critical_level", 90.0) - self.temperatureCriticalLevel = ( - 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) # Debuging ---------- self.get_logger().info('"temperature_sensor_publisher" has been started') @@ -69,7 +49,7 @@ def timer_callback(self): and publishes it to the "/auv/temperature" topic. """ # Get temperature data - self.temperature = self.Temperature.get_temperature() + self.temperature = self.temperature.get_temperature() # Publish temperature data temperature_msg = Float32() @@ -83,10 +63,8 @@ def warning_timer_callback(self): 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.temperatureCriticalLevel: - self.logger.fatal( - f"WARNING: Temperature inside the Drone to HIGH: {self.temperature} *C! Drone might be overheating!" - ) + 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!") def main(args=None): 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 e8804c906..c58dc70da 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,9 +89,7 @@ def __init__(self): 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) @@ -110,17 +108,11 @@ def __init__(self): 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")) @@ -227,36 +219,12 @@ 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_: @@ -266,12 +234,7 @@ 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 @@ -297,9 +260,7 @@ 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) 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 57622f86d..d9673594f 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 @@ -8,50 +8,48 @@ 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], ): # Initialice the I2C comunication 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}") - self.PICO_I2C_ADDRESS = PICO_I2C_ADDRESS + self.bus = smbus2.SMBus(i2c_bus) + except Exception as error_code: + print(f"ERROR: Failed connection I2C buss 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 + 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 behavious 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 + 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.ros2_package_name_for_thruster_datasheet = ros2_package_name_for_thruster_datasheet def _interpolate_forces_to_pwm(self, thruster_forces_array): """ @@ -64,8 +62,8 @@ 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", + 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)"], ) @@ -74,12 +72,12 @@ def _interpolate_forces_to_pwm(self, thruster_forces_array): thruster_forces_array[i] = thruster_forces / 9.80665 # Interpolate data - thrusterDatasheetFileForces = thrusterDatasheetFileData[" Force (Kg f)"].values - thrusterDatasheetFileDataPWM = thrusterDatasheetFileData[" PWM (µs)"].values + thruster_datasheet_file_forces = thruster_datasheet_file_data[" Force (Kg f)"].values + thruster_datasheet_file_data_pwm = thruster_datasheet_file_data[" PWM (µs)"].values interpolated_pwm = numpy.interp( thruster_forces_array, - thrusterDatasheetFileForces, - thrusterDatasheetFileDataPWM, + thruster_datasheet_file_forces, + thruster_datasheet_file_data_pwm, ) # Convert PWM signal to integers as they are interpolated and can have floats @@ -100,7 +98,7 @@ def _send_data_to_escs(self, thruster_pwm_array): # 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) + self.bus.write_i2c_block_data(self.pico_i2c_address, 0, i2c_data_array) def drive_thrusters(self, thruster_forces_array): """ @@ -120,32 +118,27 @@ 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] - ) + 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): + 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] + 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 extention the thrusters try: self._send_data_to_escs(thruster_pwm_array) - except Exception as errorCode: - print(f"ERROR: Failed to send PWM values: {errorCode}") + 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 index cb878a232..4d6fe07fa 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 @@ -20,21 +20,13 @@ def __init__(self): # Create a subscriber that takes data from thruster forces # Then convert this Forces into PWM signals and control the thrusters # Publish PWM values as deebuging feature - self.thruster_forces_subscriber = self.create_subscription( - ThrusterForces, "thrust/thruster_forces", self._thruster_forces_callback, 10 - ) + self.thruster_forces_subscriber = self.create_subscription(ThrusterForces, "thrust/thruster_forces", self._thruster_forces_callback, 10) self.thruster_pwm_publisher = self.create_publisher(Int16MultiArray, "pwm", 10) # Get thruster mapping, direction, offset and clamping parameters - self.declare_parameter( - "propulsion.thrusters.thruster_to_pin_mapping", [7, 6, 5, 4, 3, 2, 1, 0] - ) - self.declare_parameter( - "propulsion.thrusters.thruster_direction", [1, 1, 1, 1, 1, 1, 1, 1] - ) - self.declare_parameter( - "propulsion.thrusters.thruster_PWM_offset", [0, 0, 0, 0, 0, 0, 0, 0] - ) + self.declare_parameter("propulsion.thrusters.thruster_to_pin_mapping", [7, 6, 5, 4, 3, 2, 1, 0]) + self.declare_parameter("propulsion.thrusters.thruster_direction", [1, 1, 1, 1, 1, 1, 1, 1]) + self.declare_parameter("propulsion.thrusters.thruster_PWM_offset", [0, 0, 0, 0, 0, 0, 0, 0]) self.declare_parameter( "propulsion.thrusters.thruster_PWM_min", [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100], @@ -46,35 +38,21 @@ def __init__(self): self.declare_parameter("propulsion.thrusters.thrust_update_rate", 10.0) - self.thruster_mapping = self.get_parameter( - "propulsion.thrusters.thruster_to_pin_mapping" - ).value - self.thruster_direction = self.get_parameter( - "propulsion.thrusters.thruster_direction" - ).value - self.thruster_PWM_offset = self.get_parameter( - "propulsion.thrusters.thruster_PWM_offset" - ).value - self.thruster_PWM_min = self.get_parameter( - "propulsion.thrusters.thruster_PWM_min" - ).value - self.thruster_PWM_max = self.get_parameter( - "propulsion.thrusters.thruster_PWM_max" - ).value - self.thrust_timer_period = ( - 1.0 / self.get_parameter("propulsion.thrusters.thrust_update_rate").value - ) + self.thruster_mapping = self.get_parameter("propulsion.thrusters.thruster_to_pin_mapping").value + self.thruster_direction = self.get_parameter("propulsion.thrusters.thruster_direction").value + self.thruster_pwm_offset = self.get_parameter("propulsion.thrusters.thruster_PWM_offset").value + self.thruster_pwm_min = self.get_parameter("propulsion.thrusters.thruster_PWM_min").value + self.thruster_pwm_max = self.get_parameter("propulsion.thrusters.thruster_PWM_max").value + self.thrust_timer_period = 1.0 / self.get_parameter("propulsion.thrusters.thrust_update_rate").value # Initialize thruster driver self.thruster_driver = ThrusterInterfaceAUVDriver( - ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET=get_package_share_directory( - "thruster_interface_auv" - ), - 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, + 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 @@ -93,9 +71,7 @@ def _thruster_forces_callback(self, msg): def _timer_callback(self): # 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 - ) + thruster_pwm_array = self.thruster_driver.drive_thrusters(self.thruster_forces_array) pwm_message = Int16MultiArray() pwm_message.data = thruster_pwm_array From 36699f482a2e6201f8fc9908ee5f7235ba8eb92c Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 21:44:40 +0200 Subject: [PATCH 30/51] chore: update linting and project configuration rules in .pylintrc and pyproject.toml --- .pylintrc | 3 +-- pyproject.toml | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/.pylintrc b/.pylintrc index 0f2d68d5d..2cc576cb3 100644 --- a/.pylintrc +++ b/.pylintrc @@ -345,7 +345,7 @@ indent-after-paren=4 indent-string=' ' # Maximum number of characters on a single line. -max-line-length=148 +max-line-length=150 # Maximum number of lines in a module. max-module-lines=1000 @@ -433,7 +433,6 @@ 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 - invalid-name, # [C0103] Naming convention not followed (e.g., function, variable, or class names) 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 diff --git a/pyproject.toml b/pyproject.toml index 3b64841c4..833724e03 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,5 +1,5 @@ [tool.black] -line-length = 88 +line-length = 150 target-version = ['py39'] skip-string-normalization = true From d01b76bdd7d8246c0da46a70a1e2be2f2d88682b Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Sep 2024 21:53:22 +0200 Subject: [PATCH 31/51] Rename files to snake_case --- mission/LCD/sources/{IP_lib.py => ip_lib.py} | 0 mission/LCD/sources/{LCD.py => lcd.py} | 4 ++-- mission/LCD/sources/{LCD_lib.py => lcd_lib.py} | 0 3 files changed, 2 insertions(+), 2 deletions(-) rename mission/LCD/sources/{IP_lib.py => ip_lib.py} (100%) rename mission/LCD/sources/{LCD.py => lcd.py} (97%) rename mission/LCD/sources/{LCD_lib.py => lcd_lib.py} (100%) diff --git a/mission/LCD/sources/IP_lib.py b/mission/LCD/sources/ip_lib.py similarity index 100% rename from mission/LCD/sources/IP_lib.py rename to mission/LCD/sources/ip_lib.py diff --git a/mission/LCD/sources/LCD.py b/mission/LCD/sources/lcd.py similarity index 97% rename from mission/LCD/sources/LCD.py rename to mission/LCD/sources/lcd.py index 8d4ed0356..6ffc0d03c 100644 --- a/mission/LCD/sources/LCD.py +++ b/mission/LCD/sources/lcd.py @@ -2,10 +2,10 @@ # Python Libraries from time import sleep -from IP_lib import IPDriver +from ip_lib import IPDriver # Custom Libraries -from LCD_lib import LCDScreenDriver +from lcd_lib import LCDScreenDriver from power_sense_module_lib import PowerSenseModule from pressure_sensor_lib import PressureSensor from temperature_sensor_lib import TemperatureSensor diff --git a/mission/LCD/sources/LCD_lib.py b/mission/LCD/sources/lcd_lib.py similarity index 100% rename from mission/LCD/sources/LCD_lib.py rename to mission/LCD/sources/lcd_lib.py From c5eff2498583f7a37f8922fc22f627398def506b Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Tue, 24 Sep 2024 08:00:05 +0200 Subject: [PATCH 32/51] ci: add pipeline for grammar in comments --- .github/workflows/codespell.yml | 48 +++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) create mode 100644 .github/workflows/codespell.yml diff --git a/.github/workflows/codespell.yml b/.github/workflows/codespell.yml new file mode 100644 index 000000000..b575d19ab --- /dev/null +++ b/.github/workflows/codespell.yml @@ -0,0 +1,48 @@ +name: Run codespell + +on: [pull_request] + +jobs: + codespell_fix: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v4 + with: + ref: ${{ github.head_ref }} + + - name: Install codespell + run: | + sudo apt-get update + sudo apt-get install -y codespell + + - name: Run codespell to fix spelling mistakes + run: | + codespell -w . + + - name: Commit codespell changes + uses: EndBug/add-and-commit@v9 + with: + author_name: Codespell Robot + author_email: codespell-robot@example.com + message: 'Committing codespell fixes' + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + + codespell_check: + runs-on: ubuntu-latest + needs: codespell_fix + + steps: + - uses: actions/checkout@v4 + with: + ref: ${{ github.head_ref }} + + - name: Install codespell + run: | + sudo apt-get update + sudo apt-get install -y codespell + + - name: Run codespell to check for unresolved mistakes + run: | + codespell . From d7cee00ea778aa6381319e178406189a532dc2ef Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Tue, 24 Sep 2024 08:00:21 +0200 Subject: [PATCH 33/51] refactor: fix spelling mistakes in comments --- acoustics/acoustics_data_record/README | 2 +- .../acoustics_data_record_lib.py | 2 +- .../acoustics_data_record_node.py | 8 ++--- .../utilities/display_acoustics_data_live.py | 6 ++-- acoustics/acoustics_interface/README | 2 +- .../acoustics_interface_lib.py | 4 +-- .../acoustics_interface_node.py | 6 ++-- auv_setup/config/robots/beluga.yaml | 2 +- auv_setup/config/robots/manta_enu.yaml | 2 +- auv_setup/config/robots/orca.yaml | 30 +++++++++---------- mission/LCD/README | 2 +- mission/LCD/sources/lcd.py | 4 +-- mission/LCD/sources/lcd_lib.py | 2 +- mission/LCD/sources/power_sense_module_lib.py | 6 ++-- mission/LCD/sources/temperature_sensor_lib.py | 6 ++-- mission/LCD/startup_scripts/LCD.service | 4 +-- mission/blackbox/README | 2 +- .../blackbox/blackbox/blackbox_log_data.py | 6 ++-- mission/blackbox/blackbox/blackbox_node.py | 4 +-- mission/blackbox/package.xml | 2 +- .../blackbox/startup_scripts/blackbox.service | 2 +- .../power_sense_module_lib.py | 6 ++-- .../power_sense_module_node.py | 2 +- .../pressure_sensor_node.py | 2 +- .../temperature_sensor_lib.py | 6 ++-- .../temperature_sensor_node.py | 2 +- .../internal_status_auv.service | 4 +-- .../thrust_allocator_utils.hpp | 2 +- motion/thruster_interface_auv/package.xml | 2 +- .../thruster_interface_auv_driver_lib.py | 8 ++--- .../thruster_interface_auv_node.py | 2 +- .../add_service_files_to_bootup_sequence.sh | 4 +-- 32 files changed, 72 insertions(+), 72 deletions(-) diff --git a/acoustics/acoustics_data_record/README b/acoustics/acoustics_data_record/README index 5e2451090..e2543faff 100644 --- a/acoustics/acoustics_data_record/README +++ b/acoustics/acoustics_data_record/README @@ -3,4 +3,4 @@ Has the ability to display data live in real time OBS!: Make sure the Acoustics Interface Node is running before starting acoustics recording node, otherwise you will be recording nothing :P Use utilities/display_acoustics_data_live.py to display data in real time while it is being saved into the .csv file through the ROS2 node -(Verry cool to look at, would recoment ;DDD) +(Very cool to look at, would recoment ;DDD) 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 c6f3f7847..09f19bb5f 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 @@ -65,7 +65,7 @@ def log_data_to_csv_file( 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 miliseconds + # Get current time in hours, minutes, seconds and milliseconds current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] # Write to .csv file 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 099302e09..0970398d4 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 @@ -15,11 +15,11 @@ class AcousticsDataRecordNode(Node): def __init__(self): - # Variables for seting upp loging correctly + # Variables for setting upp loging correctly 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 - position_data_size = 3 # position only has X, Y, Z basicaly 3 elements + position_data_size = 3 # position only has X, Y, Z basically 3 elements # Initialize ROS2 node super().__init__("acoustics_data_record_node") @@ -80,12 +80,12 @@ def __init__(self): 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, verry slow + 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) - # Debuging ---------- + # Debugging ---------- self.get_logger().info( "Started logging data for topics: \n" "/acoustics/hydrophone1 [Int32MultiArray] \n" 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 1d36ca7c4..399ab79fe 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -12,11 +12,11 @@ # Libraries for anmation from matplotlib import animation, gridspec -# Variables for seting upp data structures correctly +# 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 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 -POSITION_DATA_SIZE = 3 # position only has X, Y, Z basicaly 3 elements +POSITION_DATA_SIZE = 3 # position only has X, Y, Z basically 3 elements # Important variables for later processing of data SAMPLE_RATE = 430_000 # 430 kHz @@ -245,7 +245,7 @@ def display_live_data(): # Get latest acoustics data acoustics_data = get_acoustics_data() - # Set the lates acoustics data in apropriate variables + # Set the lates acoustics data in appropriate variables hydrophone_data = [ acoustics_data[0], # Hydrophone 1 acoustics_data[1], # Hydrophone 2 diff --git a/acoustics/acoustics_interface/README b/acoustics/acoustics_interface/README index 25c1a8eac..ac7347bc4 100644 --- a/acoustics/acoustics_interface/README +++ b/acoustics/acoustics_interface/README @@ -1,2 +1,2 @@ -Interface to comunicate with acoustics PCB and Teensy 4.1 MCU +Interface to communicate with acoustics PCB and Teensy 4.1 MCU OBS!: Make sure to connect Teensy 4.1 MCU to the ethernet \ No newline at end of file diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index 2bb51dc4d..adf1dcfe7 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -22,7 +22,7 @@ class TeensyCommunicationUDP: _timeoutMax (int): time to wait before retrying handshake _data_string (str): buffer for received teensy data _data_target (str): the field of `acoustics_data` that is written to - acoustics_data (dict[str, list[int]]): containter for data from teensy + acoustics_data (dict[str, list[int]]): container for data from teensy Methods: -------- @@ -222,7 +222,7 @@ def _send_acknowledge_signal(cls) -> None: """ try: cls._clientSocket.sendto(cls._INITIALIZATION_MESSAGE.encode(), cls._address) - print("DEBUGING: Sent acknowledge package") + print("DEBUGGING: Sent acknowledge package") except Exception as e: print("Error from send_acknowledge_signal") print(e) diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py index a4b1d94ab..c080f5d48 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py @@ -38,7 +38,7 @@ def __init__(self) -> None: 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, verry slow + 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 @@ -60,8 +60,8 @@ def __init__(self) -> None: ) ] - # Initialize comunication with Acoustics PCB - self.get_logger().info("Initializing comunication with Acoustics") + # Initialize communication with Acoustics PCB + self.get_logger().info("Initializing communication with Acoustics") self.get_logger().info("Acoustics PCB MCU IP: 10.0.0.111") self.get_logger().info("Trying to connect...") diff --git a/auv_setup/config/robots/beluga.yaml b/auv_setup/config/robots/beluga.yaml index 0895f2ed2..da8f8e49f 100644 --- a/auv_setup/config/robots/beluga.yaml +++ b/auv_setup/config/robots/beluga.yaml @@ -104,7 +104,7 @@ controllers: odometry_topic: "/odometry/filtered" sphere_of_acceptence: 0.2 control_input_saturation_limits: [-10.5, 10.5] - control_forces_weights: [0.1, 0.1, 0.1, 10, 10, 1] # Weigths for what DOF is prioritized when allocation control forces. Leave + control_forces_weights: [0.1, 0.1, 0.1, 10, 10, 1] # Weights for what DOF is prioritized when allocation control forces. Leave control_bandwidth: [0.6, 1, 0.6, 0.6, 0.6, 0.6] # Tune the PID control bandwidths for [surge, sway, heave, roll, pitch, yaw]. NB! roll and pitch are redundant relative_damping_ratio: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # Leave absolute_relative_velocity_limit: 0.5 # Leave diff --git a/auv_setup/config/robots/manta_enu.yaml b/auv_setup/config/robots/manta_enu.yaml index 9cbc3b4ef..9d3cce60a 100644 --- a/auv_setup/config/robots/manta_enu.yaml +++ b/auv_setup/config/robots/manta_enu.yaml @@ -102,7 +102,7 @@ guidance: computer: 'odroid' -torpedos: # TODO: decide what pins to use for torpedo +torpedoes: # TODO: decide what pins to use for torpedo left_pin: 23 # both given in BCM scheme right_pin: 24 diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index f98dce738..0e72ff14d 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -45,28 +45,28 @@ thruster_to_pin_mapping: [0, 1, 2, 3, 4, 6, 5, 7] # I.e. if thruster_to_pin = [ 7, 6, 5, 4, 3, 2, 1, 0 ] then thruster 0 is pin 1 etc.. thruster_direction: [1, -1, 1, -1, 1, 1, 1, -1] # Disclose during thruster mapping (+/- 1) thruster_PWM_offset: [-28, -28, -28, -28, -28, -28, -28, -28] # Offset IN PWM -400 to 400 # Offset moves where the thrust is at rest - thruster_PWM_min: [1200, 1200, 1200, 1200, 1200, 1200, 1200, 1200] # Minimum PWM value, Recomended: [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100] - thruster_PWM_max: [1800, 1800, 1800, 1800, 1800, 1800, 1800, 1800] # Maximum PWM value, Recomended: [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900] + thruster_PWM_min: [1200, 1200, 1200, 1200, 1200, 1200, 1200, 1200] # Minimum PWM value, Recommended: [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100] + thruster_PWM_max: [1800, 1800, 1800, 1800, 1800, 1800, 1800, 1800] # Maximum PWM value, Recommended: [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900] internal_status: - power_sense_module_read_rate: 5.0 # Readings/second, Recomended: 5.0 - voltage_min: 14.5 # [V], Recomended: 14.5 - voltage_max: 16.8 # [V], Recomended: 16.8 - power_sense_module_warning_rate: 0.1 # Warnings/second in case something goes bad you have a warning, Recomended: 0.1 + power_sense_module_read_rate: 5.0 # Readings/second, Recommended: 5.0 + voltage_min: 14.5 # [V], Recommended: 14.5 + voltage_max: 16.8 # [V], Recommended: 16.8 + power_sense_module_warning_rate: 0.1 # Warnings/second in case something goes bad you have a warning, Recommended: 0.1 - pressure_read_rate: 1.0 # Readings/second, Recomended: 1.0 - pressure_critical_level: 900.0 # [hPa] If the pressure is over this amount there might be a leakage, Recomended: <1013.25 - pressure_warning_rate: 0.1 # Warnings/second in case something goes bad you have a warning, Recomended: 0.1 + pressure_read_rate: 1.0 # Readings/second, Recommended: 1.0 + pressure_critical_level: 900.0 # [hPa] If the pressure is over this amount there might be a leakage, Recommended: <1013.25 + pressure_warning_rate: 0.1 # Warnings/second in case something goes bad you have a warning, Recommended: 0.1 - temperature_read_rate: 0.1 # Readings/second, Recomended: 0.1 - temperature_critical_level: 90.0 # [*C] If temperature is over this amount the electrical housing might be overheating, Recomended: 90.0 - temperature_warning_rate: 0.1 # Warnings/second in case something goes bad you have a warning, Recomended: 0.1 + temperature_read_rate: 0.1 # Readings/second, Recommended: 0.1 + temperature_critical_level: 90.0 # [*C] If temperature is over this amount the electrical housing might be overheating, Recommended: 90.0 + temperature_warning_rate: 0.1 # Warnings/second in case something goes bad you have a warning, Recommended: 0.1 blackbox: - data_logging_rate: 5.0 # [logings/second], Recomended: 5.0 logings per second + data_logging_rate: 5.0 # [logings/second], Recommended: 5.0 logings per second acoustics: - frequencies_of_interest: # MUST be 20 elements [FREQUENCY [Hz], FREQUENCY_VARIANCE [Hz]...] Recomended: Frequency 1 000 - 50 000 Hz, Variance 1 000 - 10 000 Hz + frequencies_of_interest: # MUST be 20 elements [FREQUENCY [Hz], FREQUENCY_VARIANCE [Hz]...] Recommended: Frequency 1 000 - 50 000 Hz, Variance 1 000 - 10 000 Hz [10000, 1000, 50000, 3000, 0, 0, @@ -77,7 +77,7 @@ 0, 0, 0, 0, 0, 0] - data_logging_rate: 1.0 # [logings/second], Recomended: 1.0 logings per second + data_logging_rate: 1.0 # [logings/second], Recommended: 1.0 logings per second diff --git a/mission/LCD/README b/mission/LCD/README index 5cdd4ca7c..ccda1b85c 100644 --- a/mission/LCD/README +++ b/mission/LCD/README @@ -1,5 +1,5 @@ # LCD Screen Code -This is the code for the on board LCD screen to show status of different things withouth geting inside the RPI directly, very usefull for hardware people to trouble shoot stuff +This is the code for the on board LCD screen to show status of different things without getting inside the RPI directly, very useful for hardware people to trouble shoot stuff # LCD on startup There is a startup scipt that should run every time. If it doesnet try going to: diff --git a/mission/LCD/sources/lcd.py b/mission/LCD/sources/lcd.py index 6ffc0d03c..653d2f733 100644 --- a/mission/LCD/sources/lcd.py +++ b/mission/LCD/sources/lcd.py @@ -10,7 +10,7 @@ from pressure_sensor_lib import PressureSensor from temperature_sensor_lib import TemperatureSensor -# Initialize all necesarry drivers/libraries for the display and functionality +# Initialize all necessary drivers/libraries for the display and functionality LCD = LCDScreenDriver() IP = IPDriver() PSM = PowerSenseModule() @@ -18,7 +18,7 @@ Temperature = TemperatureSensor() -# Formating function for nices LCD screen layout +# Formatting function for nices LCD screen layout def format_line(value: str, unit: str): """ Formats a string to fit within a 16-character display, appending a unit with spacing. diff --git a/mission/LCD/sources/lcd_lib.py b/mission/LCD/sources/lcd_lib.py index bce20eb75..6b62ae8cc 100644 --- a/mission/LCD/sources/lcd_lib.py +++ b/mission/LCD/sources/lcd_lib.py @@ -65,7 +65,7 @@ def fancy_animation(self, animation_speed=0.4): The animation is displayed in two rows of the LCD screen. """ - # Calculate the apropriate animation speed + # Calculate the appropriate animation speed animation_speed = 1 / animation_speed # Custom characters ---------- diff --git a/mission/LCD/sources/power_sense_module_lib.py b/mission/LCD/sources/power_sense_module_lib.py index 477fd60bc..65ba6bd3a 100755 --- a/mission/LCD/sources/power_sense_module_lib.py +++ b/mission/LCD/sources/power_sense_module_lib.py @@ -1,4 +1,4 @@ -# Libraies for Power Sense Module +# Libraries for Power Sense Module import time import smbus @@ -17,7 +17,7 @@ def __init__(self): self.bus = smbus.SMBus(1) except Exception as error: print(f"ERROR: Failed to connect to the I2C: {error}") - time.sleep(1) # A short pause because sometimes I2C is slow to conect + time.sleep(1) # A short pause because sometimes I2C is slow to connect # Connect to the PSM through I2C self.channel_voltage = None @@ -28,7 +28,7 @@ def __init__(self): except Exception as error: print(f"ERROR: Failed connecting to PSM: {error}") - # Convertion ratios taken from PSM datasheet at: https://bluerobotics.com/store/comm-control-power/control/psm-asm-r2-rp/ + # Conversion ratios taken from PSM datasheet at: https://bluerobotics.com/store/comm-control-power/control/psm-asm-r2-rp/ self.psm_to_battery_voltage = 11.0 # V/V self.psm_to_battery_current_scale_factor = 37.8788 # A/V self.psm_to_battery_current_offset = 0.330 # V diff --git a/mission/LCD/sources/temperature_sensor_lib.py b/mission/LCD/sources/temperature_sensor_lib.py index 8484ddec9..077dd6118 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 temerature -! Instead we just use Internal Computer temperature sensor to gaugue temperature of teh enviroment aproximately -! In the future someone should implement a external temperture sensor for measuting a more acurate state of the temperatuer on the inside of the AUV +! 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 """ # Python Libraries diff --git a/mission/LCD/startup_scripts/LCD.service b/mission/LCD/startup_scripts/LCD.service index 206c5c38a..dba6cf0f1 100755 --- a/mission/LCD/startup_scripts/LCD.service +++ b/mission/LCD/startup_scripts/LCD.service @@ -5,7 +5,7 @@ After=network.target [Service] # Use the wrapper script for ExecStart # DONT CHANGE THIS LINE!!!! -# Use ./vortex-auv/add_service_files_to_bootup_sequence.sh to automaticaly set everything up +# Use ./vortex-auv/add_service_files_to_bootup_sequence.sh to automatically set everything up ExecStart=/bin/bash -c 'python3 ../sources/LCD.py' WorkingDirectory=/home/vortex/ StandardOutput=journal+console @@ -14,7 +14,7 @@ Group=vortex Restart=no RestartSec=2 # DONT CHANGE THIS LINE!!!! -# Use ./vortex-auv/add_service_files_to_bootup_sequence.sh to automaticaly set everything up +# Use ./vortex-auv/add_service_files_to_bootup_sequence.sh to automatically set everything up Environment="PYTHONPATH=/usr/local/lib//dist-packages:$PYTHONPATH" [Install] diff --git a/mission/blackbox/README b/mission/blackbox/README index 4cb923305..cec230a53 100644 --- a/mission/blackbox/README +++ b/mission/blackbox/README @@ -11,7 +11,7 @@ Logs data of all the important topics such as: ## Service file bootup -To start the blackbox loging automaticaly every time on bootup just run this command: +To start the blackbox loging automatically every time on bootup just run this command: ``` ./vortex-auv/add_service_files_to_bootup_sequence.sh ``` diff --git a/mission/blackbox/blackbox/blackbox_log_data.py b/mission/blackbox/blackbox/blackbox_log_data.py index 6642d46ad..a05c114da 100755 --- a/mission/blackbox/blackbox/blackbox_log_data.py +++ b/mission/blackbox/blackbox/blackbox_log_data.py @@ -43,8 +43,8 @@ def __init__(self, ros2_package_directory=""): ] # Manage csv files for blackbox data ---------- - # If there are stale old .csv files => Delete oldes ones - # If .csv files take up to much space => Delte oldest ones + # If there are stale old .csv files => Delete oldest ones + # If .csv files take up to much space => Delete oldest ones self.manage_csv_files() # Make new .csv file for loging blackbox data ---------- @@ -190,7 +190,7 @@ def log_data_to_csv_file( This method appends a new row to the CSV file specified by `self.data_file_location`. The row contains the current time and the provided data values. """ - # Get current time in hours, minutes, seconds and miliseconds + # Get current time in hours, minutes, seconds and milliseconds current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] # Write to .csv file diff --git a/mission/blackbox/blackbox/blackbox_node.py b/mission/blackbox/blackbox/blackbox_node.py index fd4e6737e..62a3a344e 100755 --- a/mission/blackbox/blackbox/blackbox_node.py +++ b/mission/blackbox/blackbox/blackbox_node.py @@ -49,12 +49,12 @@ def __init__(self): 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, verry slow + 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) - # Debuging ---------- + # Debugging ---------- self.get_logger().info( "Started logging data for topics: \n" "/auv/power_sense_module/current [Float32] \n" diff --git a/mission/blackbox/package.xml b/mission/blackbox/package.xml index 3d420c61c..0242798a3 100644 --- a/mission/blackbox/package.xml +++ b/mission/blackbox/package.xml @@ -3,7 +3,7 @@ blackbox 1.0.0 - Logs all ROS2 data and other internal statuses to a data file for use in analizing data latern + Logs all ROS2 data and other internal statuses to a data file for use in analyzing data latern vortex MIT diff --git a/mission/blackbox/startup_scripts/blackbox.service b/mission/blackbox/startup_scripts/blackbox.service index 62d30e3dd..fa79d6c32 100644 --- a/mission/blackbox/startup_scripts/blackbox.service +++ b/mission/blackbox/startup_scripts/blackbox.service @@ -5,7 +5,7 @@ After=network.target [Service] # Use the wrapper script for ExecStart # DONT CHANGE THIS LINE!!!! -# Use ./vortex-auv/add_service_files_to_bootup_sequence.sh to automaticaly set everything up +# Use ./vortex-auv/add_service_files_to_bootup_sequence.sh to automatically set everything up ExecStart=/bin/bash 'blackbox.sh' WorkingDirectory=/home/vortex/ StandardOutput=journal+console 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 596f95ead..651e46acb 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 @@ -1,4 +1,4 @@ -# Libraies for Power Sense Module +# Libraries for Power Sense Module import time import smbus @@ -17,7 +17,7 @@ def __init__(self): self.bus = smbus.SMBus(1) except Exception as error: print(f"ERROR: Failed to connect to the I2C: {error}") - time.sleep(1) # A short pause because sometimes I2C is slow to conect + time.sleep(1) # A short pause because sometimes I2C is slow to connect # Connect to the PSM through I2C self.channel_voltage = None @@ -28,7 +28,7 @@ def __init__(self): except Exception as error: print(f"ERROR: Failed connecting to PSM: {error}") - # Convertion ratios taken from PSM datasheet at: https://bluerobotics.com/store/comm-control-power/control/psm-asm-r2-rp/ + # Conversion ratios taken from PSM datasheet at: https://bluerobotics.com/store/comm-control-power/control/psm-asm-r2-rp/ self.psm_to_battery_voltage = 11.0 # V/V self.psm_to_battery_current_scale_factor = 37.8788 # A/V self.psm_to_battery_current_offset = 0.330 # V 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 920bf13ad..976f2baf6 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 @@ -43,7 +43,7 @@ def __init__(self): warning_timer_period = 1.0 / warning_rate self.warning_timer = self.create_timer(warning_timer_period, self.warning_timer_callback) - # Debuging ---------- + # Debugging ---------- self.get_logger().info('"power_sense_module_publisher" has been started') def read_timer_callback(self): 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 5a8c7b84f..90414efb4 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 @@ -38,7 +38,7 @@ def __init__(self): warning_timer_period = 1.0 / warning_rate self.warning_timer = self.create_timer(warning_timer_period, self.warning_timer_callback) - # Debuging ---------- + # Debugging ---------- self.get_logger().info('"pressure_sensor_publisher" has been started') def timer_callback(self): 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 467c33d97..35c3a45d5 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 temerature -! Instead we just use Internal Computer temperature sensor to gaugue temperature of teh enviroment aproximately -! In the future someone should implement a external temperture sensor for measuting a more acurate state of the temperatuer on the inside of the AUV +! 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 """ # Python Libraries 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 45a862fc1..5500c4421 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 @@ -38,7 +38,7 @@ def __init__(self): warning_timer_period = 1.0 / warning_rate self.warning_timer = self.create_timer(warning_timer_period, self.warning_timer_callback) - # Debuging ---------- + # Debugging ---------- self.get_logger().info('"temperature_sensor_publisher" has been started') def timer_callback(self): diff --git a/mission/internal_status_auv/startup_scripts/internal_status_auv.service b/mission/internal_status_auv/startup_scripts/internal_status_auv.service index 240030ff0..184cb922d 100644 --- a/mission/internal_status_auv/startup_scripts/internal_status_auv.service +++ b/mission/internal_status_auv/startup_scripts/internal_status_auv.service @@ -5,7 +5,7 @@ After=network.target [Service] # Use the wrapper script for ExecStart # DONT CHANGE THIS LINE!!!! -# Use ./vortex-auv/add_service_files_to_bootup_sequence.sh to automaticaly set everything up +# Use ./vortex-auv/add_service_files_to_bootup_sequence.sh to automatically set everything up ExecStart=/bin/bash 'internal_status_auv.sh' WorkingDirectory=/home/vortex/ StandardOutput=journal+console @@ -13,7 +13,7 @@ User=vortex Restart=no RestartSec=2 # DONT CHANGE THIS LINE!!!! -# Use ./vortex-auv/add_service_files_to_bootup_sequence.sh to automaticaly set everything up +# Use ./vortex-auv/add_service_files_to_bootup_sequence.sh to automatically set everything up Environment="PYTHONPATH=/usr/local/lib//dist-packages:$PYTHONPATH" [Install] 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 bf6848bcf..8009f30d8 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 @@ -64,7 +64,7 @@ inline Eigen::MatrixXd calculate_thrust_allocation_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 Psuedoinverse Calculated"); + throw std::runtime_error("Invalid Pseudoinverse Calculated"); } return pseudoinverse; } diff --git a/motion/thruster_interface_auv/package.xml b/motion/thruster_interface_auv/package.xml index bce547c07..c4b756d19 100644 --- a/motion/thruster_interface_auv/package.xml +++ b/motion/thruster_interface_auv/package.xml @@ -3,7 +3,7 @@ thruster_interface_auv 1.0.0 - Thruster interface to controll thrusters through PCA9685 Module + Thruster interface to control thrusters through PCA9685 Module vortex MIT 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 d9673594f..9a93948f1 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,12 +18,12 @@ def __init__( pwm_min=[1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100], pwm_max=[1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900], ): - # Initialice the I2C comunication + # 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 buss nr {self.bus}: {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 @@ -110,7 +110,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: @@ -135,7 +135,7 @@ 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 + # 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: 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 4d6fe07fa..99a3c8245 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 @@ -70,7 +70,7 @@ def _thruster_forces_callback(self, msg): def _timer_callback(self): # 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 + # 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() diff --git a/scripts/add_service_files_to_bootup_sequence.sh b/scripts/add_service_files_to_bootup_sequence.sh index 3c07db527..3451b687b 100755 --- a/scripts/add_service_files_to_bootup_sequence.sh +++ b/scripts/add_service_files_to_bootup_sequence.sh @@ -22,7 +22,7 @@ cd $SCRIPT_DIR # Setup ---------- -echo "Seting up .service files..." +echo "Setting up .service files..." # Copy the .service files to current directory cp $SERVICE_FILE_PATH_LCD$SERVICE_FILE_NAME_LCD $SERVICE_FILE_NAME_LCD # LCD cp $SERVICE_FILE_PATH_INTERNAL_STATUS_AUV$SERVICE_FILE_NAME_INTERNAL_STATUS_AUV $SERVICE_FILE_NAME_INTERNAL_STATUS_AUV # Internal Status @@ -54,7 +54,7 @@ rm $SERVICE_FILE_NAME_LCD # LCD rm $SERVICE_FILE_NAME_INTERNAL_STATUS_AUV # Internal Status rm $SERVICE_FILE_NAME_BLACKBOX # Blackbox -# Change permision of the .service file +# Change permission of the .service file sudo chmod 777 $SYSTEMD_PATH$SERVICE_FILE_NAME_LCD # LCD sudo chmod 777 $SYSTEMD_PATH$SERVICE_FILE_NAME_INTERNAL_STATUS_AUV # Internal Status sudo chmod 777 $SYSTEMD_PATH$SERVICE_FILE_NAME_BLACKBOX # Blackbox From 399294c438427df21f29cc45581778d86cfb407c Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Tue, 24 Sep 2024 08:02:50 +0200 Subject: [PATCH 34/51] ci: update job codespell_fix to continue on error --- .github/workflows/codespell.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/codespell.yml b/.github/workflows/codespell.yml index b575d19ab..7c15e080b 100644 --- a/.github/workflows/codespell.yml +++ b/.github/workflows/codespell.yml @@ -19,6 +19,7 @@ jobs: - name: Run codespell to fix spelling mistakes run: | codespell -w . + continue-on-error: true - name: Commit codespell changes uses: EndBug/add-and-commit@v9 From 369d7d63e1df906b2631968c59d1a16c57b2c461 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Tue, 24 Sep 2024 08:06:09 +0200 Subject: [PATCH 35/51] refactor: fix spelling mistakes in comments --- README.md | 2 +- .../acoustics_data_record/acoustics_data_record_lib.py | 2 +- .../acoustics_data_record/acoustics_data_record_node.py | 6 +++--- .../utilities/display_acoustics_data_live.py | 4 ++-- auv_setup/config/robots/beluga.yaml | 2 +- mission/LCD/README | 4 ++-- mission/blackbox/README | 4 ++-- mission/blackbox/blackbox/blackbox_log_data.py | 2 +- mission/blackbox/blackbox/blackbox_node.py | 2 +- .../thruster_interface_auv_driver_lib.py | 2 +- 10 files changed, 15 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 9d57381db..3ab8718c3 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,7 @@ This repo contains software for operating UUVs, developed by students at NTNU. T ## Documentation * TODO: Drivers and hardware specifics for each drone will be added to the wiki. Link them here. * TODO: How to adapt the software stack to new hardware. -* A collection of master theses written by Vortex members: +* A collection of master thesis written by Vortex members: * [Manta v1: A Deliberative Agent Software Architecture for Autonomous Underwater Vehicles](https://github.com/vortexntnu/Vortex-AUV/blob/documentation/top-level_readme/docs/master_theses/Kristoffer%20Solberg%20(2020).pdf) * [A real-time DVL and pressure sensor AINS comparison study between EKF, ESKF and NLO for Manta-2020](https://github.com/vortexntnu/Vortex-AUV/blob/documentation/top-level_readme/docs/master_theses/Oyvind%20Denvik%20(2020).pdf) * [Sonar EKF-SLAM and mapping inanstructured underwater environment](https://github.com/vortexntnu/Vortex-AUV/blob/documentation/top-level_readme/docs/master_theses/Ambj%C3%B8rn%20Waldum%20(2020).pdf) 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 09f19bb5f..40ecf836f 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 @@ -28,7 +28,7 @@ def __init__(self, ros2_package_directory=""): "Position", ] - # Make new .csv file for loging blackbox data ---------- + # Make new .csv file for logging blackbox data ---------- 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) 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 0970398d4..6d001a46d 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 @@ -15,7 +15,7 @@ class AcousticsDataRecordNode(Node): def __init__(self): - # Variables for setting upp loging correctly + # 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 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 @@ -76,7 +76,7 @@ def __init__(self): ros2_package_directory_location + "src/vortex-auv/acoustics/acoustics_data_record/" ) # Navigate to this package - # Make blackbox loging file + # Make blackbox logging file self.acoustics_data_record = AcousticsDataRecordLib(ros2_package_directory=ros2_package_directory_location) # Logs all the newest data 1 time(s) per second @@ -233,7 +233,7 @@ def main(): acoustics_data_record_node = AcousticsDataRecordNode() rclpy.spin(acoustics_data_record_node) - # Destroy the node explicitly once ROS2 stops runing + # Destroy the node explicitly once ROS2 stops running acoustics_data_record_node.destroy_node() rclpy.shutdown() 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 399ab79fe..048bd4235 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -157,7 +157,7 @@ def get_acoustics_data(): # 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 # Hydrohones have a ring buffer the size of 3 buffers each containing 1024 values (2^10) - # We always use the first ring buffer of Hydrophone 1 to performe DSP + # We always use the first ring buffer of Hydrophone 1 to perform DSP # That is why unfiltered data is the same as Hydrphne 1 first buffer unfiltered_data = hydrophone1[0:1024] @@ -245,7 +245,7 @@ def display_live_data(): # Get latest acoustics data acoustics_data = get_acoustics_data() - # Set the lates acoustics data in appropriate variables + # Set the latest acoustics data in appropriate variables hydrophone_data = [ acoustics_data[0], # Hydrophone 1 acoustics_data[1], # Hydrophone 2 diff --git a/auv_setup/config/robots/beluga.yaml b/auv_setup/config/robots/beluga.yaml index da8f8e49f..fe078e15a 100644 --- a/auv_setup/config/robots/beluga.yaml +++ b/auv_setup/config/robots/beluga.yaml @@ -191,7 +191,7 @@ pressure: critical: 1013.25 # hPa system: interval: 0.5 # seconds - loger: + logger: interval: 10 # seconds temperature: diff --git a/mission/LCD/README b/mission/LCD/README index ccda1b85c..6e6efb275 100644 --- a/mission/LCD/README +++ b/mission/LCD/README @@ -2,7 +2,7 @@ This is the code for the on board LCD screen to show status of different things without getting inside the RPI directly, very useful for hardware people to trouble shoot stuff # LCD on startup -There is a startup scipt that should run every time. If it doesnet try going to: +There is a startup script that should run every time. If it doesnet try going to: vortex-auv/scripts/ -and execute script that adds our LCD screen srit into the bootup sequence: +and execute script that adds our LCD screen srit into the boot-up sequence: ./vortex-auv/add_service_files_to_bootup_sequence.sh \ No newline at end of file diff --git a/mission/blackbox/README b/mission/blackbox/README index cec230a53..7689792fb 100644 --- a/mission/blackbox/README +++ b/mission/blackbox/README @@ -9,9 +9,9 @@ Logs data of all the important topics such as: /pwm -## Service file bootup +## Service file boot-up -To start the blackbox loging automatically every time on bootup just run this command: +To start the blackbox logging automatically every time on boot-up just run this command: ``` ./vortex-auv/add_service_files_to_bootup_sequence.sh ``` diff --git a/mission/blackbox/blackbox/blackbox_log_data.py b/mission/blackbox/blackbox/blackbox_log_data.py index a05c114da..d8042b0b2 100755 --- a/mission/blackbox/blackbox/blackbox_log_data.py +++ b/mission/blackbox/blackbox/blackbox_log_data.py @@ -47,7 +47,7 @@ def __init__(self, ros2_package_directory=""): # If .csv files take up to much space => Delete oldest ones self.manage_csv_files() - # Make new .csv file for loging blackbox data ---------- + # Make new .csv file for logging blackbox data ---------- 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) diff --git a/mission/blackbox/blackbox/blackbox_node.py b/mission/blackbox/blackbox/blackbox_node.py index 62a3a344e..73c6f13fd 100755 --- a/mission/blackbox/blackbox/blackbox_node.py +++ b/mission/blackbox/blackbox/blackbox_node.py @@ -45,7 +45,7 @@ def __init__(self): 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 loging file + # Make blackbox logging file self.blackbox_log_data = BlackBoxLogData(ros2_package_directory=ros2_package_directory_location) # Logs all the newest data 10 times per second 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 9a93948f1..5a6537aaa 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 @@ -34,7 +34,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 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: From cd305d53a8e8ee73b1e9acf5307069da80af9ecc Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Tue, 24 Sep 2024 08:08:23 +0200 Subject: [PATCH 36/51] ci: update pipeline codespell to only have one job --- .github/workflows/codespell.yml | 23 ++----------------- .../utilities/display_acoustics_data_live.py | 2 +- 2 files changed, 3 insertions(+), 22 deletions(-) diff --git a/.github/workflows/codespell.yml b/.github/workflows/codespell.yml index 7c15e080b..3154bfb7f 100644 --- a/.github/workflows/codespell.yml +++ b/.github/workflows/codespell.yml @@ -3,7 +3,7 @@ name: Run codespell on: [pull_request] jobs: - codespell_fix: + codespell: runs-on: ubuntu-latest steps: @@ -19,7 +19,6 @@ jobs: - name: Run codespell to fix spelling mistakes run: | codespell -w . - continue-on-error: true - name: Commit codespell changes uses: EndBug/add-and-commit@v9 @@ -28,22 +27,4 @@ jobs: author_email: codespell-robot@example.com message: 'Committing codespell fixes' env: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - - codespell_check: - runs-on: ubuntu-latest - needs: codespell_fix - - steps: - - uses: actions/checkout@v4 - with: - ref: ${{ github.head_ref }} - - - name: Install codespell - run: | - sudo apt-get update - sudo apt-get install -y codespell - - - name: Run codespell to check for unresolved mistakes - run: | - codespell . + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} \ No newline at end of file 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 048bd4235..6967cd2b4 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -154,7 +154,7 @@ def get_acoustics_data(): 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 + # Unfiltered data is spcial as it is the same as Hydrohone 1 first 1024 values # This is because Acoustics PCB uses Hydrophone 1 to perform DSP # Hydrohones have a ring buffer the size of 3 buffers each containing 1024 values (2^10) # We always use the first ring buffer of Hydrophone 1 to perform DSP From 377f8c126556c4bc7af161297bbcab0b0a7ff8c2 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Tue, 24 Sep 2024 08:29:28 +0200 Subject: [PATCH 37/51] refactor: fix variable naming issues and adjust imports as per review --- .../acoustics_data_record/acoustics_data_record_node.py | 6 ++---- .../utilities/display_acoustics_data_live.py | 2 +- mission/LCD/sources/lcd.py | 1 + 3 files changed, 4 insertions(+), 5 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 6d001a46d..934497f6a 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 @@ -1,12 +1,10 @@ #!/usr/bin/env python3 -# ROS2 libraries -# Python libraries +# Python Libraries import array +# ROS2 Libraries import rclpy - -# Custom libraries from acoustics_data_record_lib import AcousticsDataRecordLib from ament_index_python.packages import get_package_share_directory from rclpy.node import Node 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 6967cd2b4..0c94c98a8 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -9,7 +9,7 @@ # Libraries for handling data structures import pandas as pd -# Libraries for anmation +# Libraries for animation from matplotlib import animation, gridspec # Variables for setting upp data structures correctly diff --git a/mission/LCD/sources/lcd.py b/mission/LCD/sources/lcd.py index 653d2f733..32e268d91 100644 --- a/mission/LCD/sources/lcd.py +++ b/mission/LCD/sources/lcd.py @@ -1,4 +1,5 @@ #!/usr/bin/python3 + # Python Libraries from time import sleep From 8497150f8092ce05ad4ac9c8ca9c21242649b653 Mon Sep 17 00:00:00 2001 From: Codespell Robot Date: Tue, 24 Sep 2024 06:30:30 +0000 Subject: [PATCH 38/51] Committing codespell fixes --- .../utilities/display_acoustics_data_live.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 0c94c98a8..11799b990 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -154,7 +154,7 @@ def get_acoustics_data(): 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 spcial as it is the same as Hydrohone 1 first 1024 values + # 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 # Hydrohones have a ring buffer the size of 3 buffers each containing 1024 values (2^10) # We always use the first ring buffer of Hydrophone 1 to perform DSP From b01102a1c2640e3b31a4277066a650618ea1268f Mon Sep 17 00:00:00 2001 From: Sondre Haugen Date: Tue, 24 Sep 2024 21:11:49 +0200 Subject: [PATCH 39/51] refactor: spelling correction --- README.md | 2 +- .../acoustics_data_record_node.py | 30 +++++++++---------- .../acoustics_interface_node.py | 2 +- auv_setup/config/robots/orca.yaml | 4 +-- mission/blackbox/package.xml | 2 +- 5 files changed, 20 insertions(+), 20 deletions(-) diff --git a/README.md b/README.md index 3ab8718c3..9d57381db 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,7 @@ This repo contains software for operating UUVs, developed by students at NTNU. T ## Documentation * TODO: Drivers and hardware specifics for each drone will be added to the wiki. Link them here. * TODO: How to adapt the software stack to new hardware. -* A collection of master thesis written by Vortex members: +* A collection of master theses written by Vortex members: * [Manta v1: A Deliberative Agent Software Architecture for Autonomous Underwater Vehicles](https://github.com/vortexntnu/Vortex-AUV/blob/documentation/top-level_readme/docs/master_theses/Kristoffer%20Solberg%20(2020).pdf) * [A real-time DVL and pressure sensor AINS comparison study between EKF, ESKF and NLO for Manta-2020](https://github.com/vortexntnu/Vortex-AUV/blob/documentation/top-level_readme/docs/master_theses/Oyvind%20Denvik%20(2020).pdf) * [Sonar EKF-SLAM and mapping inanstructured underwater environment](https://github.com/vortexntnu/Vortex-AUV/blob/documentation/top-level_readme/docs/master_theses/Ambj%C3%B8rn%20Waldum%20(2020).pdf) 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 934497f6a..4d88a1492 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 @@ -25,19 +25,19 @@ def __init__(self): # Initialize Subscribers ---------- # Start listening to Hydrophone data self.subscriber_hydrophone1 = self.create_subscription(Int32MultiArray, "/acoustics/hydrophone1", self.hydrophone1_callback, 5) - self.hydropone1_data = array.array("i", [0] * hydrophone_data_size) + self.hydrophone1_data = array.array("i", [0] * hydrophone_data_size) self.subscriber_hydrophone2 = self.create_subscription(Int32MultiArray, "/acoustics/hydrophone2", self.hydrophone2_callback, 5) - self.hydropone2_data = array.array("i", [0] * hydrophone_data_size) + self.hydrophone2_data = array.array("i", [0] * hydrophone_data_size) self.subscriber_hydrophone3 = self.create_subscription(Int32MultiArray, "/acoustics/hydrophone3", self.hydrophone3_callback, 5) - self.hydropone3_data = array.array("i", [0] * hydrophone_data_size) + self.hydrophone3_data = array.array("i", [0] * hydrophone_data_size) self.subscriber_hydrophone4 = self.create_subscription(Int32MultiArray, "/acoustics/hydrophone4", self.hydrophone4_callback, 5) - self.hydropone4_data = array.array("i", [0] * hydrophone_data_size) + self.hydrophone4_data = array.array("i", [0] * hydrophone_data_size) self.subscriber_hydrophone5 = self.create_subscription(Int32MultiArray, "/acoustics/hydrophone5", self.hydrophone5_callback, 5) - self.hydropone5_data = array.array("i", [0] * hydrophone_data_size) + self.hydrophone5_data = array.array("i", [0] * hydrophone_data_size) # Start listening to DSP (Digital Signal Processing) data self.subscriber_filter_response = self.create_subscription( @@ -106,7 +106,7 @@ def hydrophone1_callback(self, msg): Args: msg (Int32MultiArray): Message containing hydrophone1 data. """ - self.hydropone1_data = msg.data + self.hydrophone1_data = msg.data def hydrophone2_callback(self, msg): """ @@ -115,7 +115,7 @@ def hydrophone2_callback(self, msg): Args: msg (Int32MultiArray): Message containing hydrophone2 data. """ - self.hydropone2_data = msg.data + self.hydrophone2_data = msg.data def hydrophone3_callback(self, msg): """ @@ -124,7 +124,7 @@ def hydrophone3_callback(self, msg): Args: msg (Int32MultiArray): Message containing hydrophone3 data. """ - self.hydropone3_data = msg.data + self.hydrophone3_data = msg.data def hydrophone4_callback(self, msg): """ @@ -133,7 +133,7 @@ def hydrophone4_callback(self, msg): Args: msg (Int32MultiArray): Message containing hydrophone4 data. """ - self.hydropone4_data = msg.data + self.hydrophone4_data = msg.data def hydrophone5_callback(self, msg): """ @@ -142,7 +142,7 @@ def hydrophone5_callback(self, msg): Args: msg (Int32MultiArray): Message containing hydrophone5 data. """ - self.hydropone5_data = msg.data + self.hydrophone5_data = msg.data def filter_response_callback(self, msg): """ @@ -197,11 +197,11 @@ def logger(self): This method is called periodically based on the data logging rate. """ self.acoustics_data_record.log_data_to_csv_file( - hydrophone1=self.hydropone1_data, - hydrophone2=self.hydropone2_data, - hydrophone3=self.hydropone3_data, - hydrophone4=self.hydropone4_data, - hydrophone5=self.hydropone5_data, + hydrophone1=self.hydrophone1_data, + hydrophone2=self.hydrophone2_data, + hydrophone3=self.hydrophone3_data, + hydrophone4=self.hydrophone4_data, + hydrophone5=self.hydrophone5_data, filter_response=self.filter_response_data, fft=self.fft_data, peaks=self.peaks_data, diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py index c080f5d48..264f8367b 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py @@ -67,7 +67,7 @@ def __init__(self) -> None: TeensyCommunicationUDP.init_communication(frequencies_of_interest) - self.get_logger().info("Sucsefully connected to Acoustics PCB MCU :D") + self.get_logger().info("Successfully connected to Acoustics PCB MCU :D") def data_update(self) -> None: """ diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index 0e72ff14d..e2ba058e5 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -63,7 +63,7 @@ temperature_warning_rate: 0.1 # Warnings/second in case something goes bad you have a warning, Recommended: 0.1 blackbox: - data_logging_rate: 5.0 # [logings/second], Recommended: 5.0 logings per second + data_logging_rate: 5.0 # [loggings/second], Recommended: 5.0 loggings per second acoustics: frequencies_of_interest: # MUST be 20 elements [FREQUENCY [Hz], FREQUENCY_VARIANCE [Hz]...] Recommended: Frequency 1 000 - 50 000 Hz, Variance 1 000 - 10 000 Hz @@ -77,7 +77,7 @@ 0, 0, 0, 0, 0, 0] - data_logging_rate: 1.0 # [logings/second], Recommended: 1.0 logings per second + data_logging_rate: 1.0 # [loggings/second], Recommended: 1.0 loggings per second diff --git a/mission/blackbox/package.xml b/mission/blackbox/package.xml index 0242798a3..b2773f550 100644 --- a/mission/blackbox/package.xml +++ b/mission/blackbox/package.xml @@ -3,7 +3,7 @@ blackbox 1.0.0 - Logs all ROS2 data and other internal statuses to a data file for use in analyzing data latern + Logs all ROS2 data and other internal statuses to a data file for use in analyzing data later vortex MIT From 428ede3e98a76507c1f8a81acf52f92c03d93c1c Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Tue, 24 Sep 2024 21:07:38 +0200 Subject: [PATCH 40/51] ci: update CI file and job names for consistency --- .../{clang-formatter.yml => clang-formatter.yaml} | 0 .github/workflows/{codespell.yml => codespell.yaml} | 2 +- ...ython-format-lint.yml => isort-black-pylint.yaml} | 12 ++++++------ 3 files changed, 7 insertions(+), 7 deletions(-) rename .github/workflows/{clang-formatter.yml => clang-formatter.yaml} (100%) rename .github/workflows/{codespell.yml => codespell.yaml} (97%) rename .github/workflows/{python-format-lint.yml => isort-black-pylint.yaml} (92%) diff --git a/.github/workflows/clang-formatter.yml b/.github/workflows/clang-formatter.yaml similarity index 100% rename from .github/workflows/clang-formatter.yml rename to .github/workflows/clang-formatter.yaml diff --git a/.github/workflows/codespell.yml b/.github/workflows/codespell.yaml similarity index 97% rename from .github/workflows/codespell.yml rename to .github/workflows/codespell.yaml index 3154bfb7f..ee92c29a6 100644 --- a/.github/workflows/codespell.yml +++ b/.github/workflows/codespell.yaml @@ -1,4 +1,4 @@ -name: Run codespell +name: codespell on: [pull_request] diff --git a/.github/workflows/python-format-lint.yml b/.github/workflows/isort-black-pylint.yaml similarity index 92% rename from .github/workflows/python-format-lint.yml rename to .github/workflows/isort-black-pylint.yaml index f29de2627..50148849d 100644 --- a/.github/workflows/python-format-lint.yml +++ b/.github/workflows/isort-black-pylint.yaml @@ -1,9 +1,9 @@ -name: Run Python formatters and linters +name: isort_black_pylint on: [pull_request] jobs: - isort_format: + isort: runs-on: ubuntu-latest steps: @@ -23,9 +23,9 @@ jobs: env: GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - black_format: + black: runs-on: ubuntu-latest - needs: isort_format + needs: isort steps: - uses: actions/checkout@v4 @@ -47,9 +47,9 @@ jobs: env: GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - pylint_check: + pylint: runs-on: ubuntu-latest - needs: black_format + needs: black strategy: matrix: python-version: ["3.9", "3.10", "3.11"] From f26643a91d894e360f62c5ffb2e1fad5ed373a0e Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Tue, 24 Sep 2024 21:16:59 +0200 Subject: [PATCH 41/51] ci: update CI file for more clarity --- .../{isort-black-pylint.yaml => python-format-lint.yaml} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename .github/workflows/{isort-black-pylint.yaml => python-format-lint.yaml} (96%) diff --git a/.github/workflows/isort-black-pylint.yaml b/.github/workflows/python-format-lint.yaml similarity index 96% rename from .github/workflows/isort-black-pylint.yaml rename to .github/workflows/python-format-lint.yaml index 50148849d..fc283e62a 100644 --- a/.github/workflows/isort-black-pylint.yaml +++ b/.github/workflows/python-format-lint.yaml @@ -1,4 +1,4 @@ -name: isort_black_pylint +name: Python Format and Lint (isort, black, pylint) on: [pull_request] From de9c3defffc20d444f97843763201e250ec0bdef Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Tue, 24 Sep 2024 21:39:53 +0200 Subject: [PATCH 42/51] chore: add codespell configuration and ignore list --- .codespellignore | 1 + .codespellrc | 6 ++++++ 2 files changed, 7 insertions(+) create mode 100644 .codespellignore create mode 100644 .codespellrc diff --git a/.codespellignore b/.codespellignore new file mode 100644 index 000000000..7688ab24d --- /dev/null +++ b/.codespellignore @@ -0,0 +1 @@ +theses \ No newline at end of file diff --git a/.codespellrc b/.codespellrc new file mode 100644 index 000000000..e3368847e --- /dev/null +++ b/.codespellrc @@ -0,0 +1,6 @@ +[codespell] +# File containing words to ignore during the spell check. +ignore-words = .codespellignore + +# Check file names as well. +check-filenames = true From 99b8d20733366c468aae61449b689c26bc1babf3 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Wed, 25 Sep 2024 13:00:56 +0200 Subject: [PATCH 43/51] chore: add type hints and return types --- .../acoustics_data_record_lib.py | 25 +++++----- .../acoustics_data_record_node.py | 26 +++++------ .../launch/acoustics_data_record.launch.py | 2 +- .../utilities/display_acoustics_data_live.py | 8 ++-- .../acoustics_interface_node.py | 2 +- .../launch/acoustics_interface_launch.py | 2 +- auv_setup/launch/orca.launch.py | 2 +- auv_setup/launch/topside.launch.py | 2 +- mission/LCD/sources/ip_lib.py | 4 +- mission/LCD/sources/lcd.py | 2 +- mission/LCD/sources/lcd_lib.py | 6 +-- mission/LCD/sources/power_sense_module_lib.py | 6 +-- mission/LCD/sources/pressure_sensor_lib.py | 4 +- mission/LCD/sources/temperature_sensor_lib.py | 4 +- .../blackbox/blackbox/blackbox_log_data.py | 46 +++++++++---------- mission/blackbox/blackbox/blackbox_node.py | 18 ++++---- mission/blackbox/launch/blackbox.launch.py | 2 +- .../power_sense_module_lib.py | 6 +-- .../power_sense_module_node.py | 8 ++-- .../pressure_sensor_lib.py | 4 +- .../pressure_sensor_node.py | 8 ++-- .../temperature_sensor_lib.py | 4 +- .../temperature_sensor_node.py | 8 ++-- .../launch/internal_status_auv.launch.py | 2 +- .../joystick_interface_auv.py | 8 ++-- .../launch/joystick_interface_auv.launch.py | 2 +- .../tests/test_joystick_interface_auv.py | 8 ++-- .../launch/thrust_allocator_auv.launch.py | 2 +- .../launch/thruster_interface_auv.launch.py | 2 +- .../thruster_interface_auv_driver_lib.py | 28 +++++------ .../thruster_interface_auv_node.py | 8 ++-- 31 files changed, 131 insertions(+), 128 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 40ecf836f..0fdb53b07 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 @@ -2,10 +2,11 @@ import csv import time from datetime import datetime +from typing import List class AcousticsDataRecordLib: - def __init__(self, ros2_package_directory=""): + def __init__(self, ros2_package_directory: str = "") -> None: # Global variables for .csv file manipulation ---------- # Get the path for the directory where we will store our data self.acoustics_data_directory = ros2_package_directory + "acoustics_data/" @@ -36,17 +37,17 @@ def __init__(self, ros2_package_directory=""): # Methods for external uses ---------- def log_data_to_csv_file( self, - hydrophone1=[0], - hydrophone2=[0], - hydrophone3=[0], - hydrophone4=[0], - hydrophone5=[0], - filter_response=[0], - fft=[0], - peaks=[0], - tdoa=[0.0], - position=[0.0], - ): + hydrophone1: List[int] = [0], + hydrophone2: List[int] = [0], + hydrophone3: List[int] = [0], + hydrophone4: List[int] = [0], + hydrophone5: List[int] = [0], + filter_response: List[int] = [0], + fft: List[int] = [0], + peaks: List[int] = [0], + tdoa: List[float] = [0.0], + position: List[float] = [0.0], + ) -> None: """ Logs the provided data to a CSV file. 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 4d88a1492..86f7d1ae2 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 @@ -12,7 +12,7 @@ class AcousticsDataRecordNode(Node): - def __init__(self): + 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 dsp_data_size = 2**10 # DSP (Digital Signal Processing) has 2^10 long data @@ -99,7 +99,7 @@ def __init__(self): ) # Callback methods for different topics - def hydrophone1_callback(self, msg): + def hydrophone1_callback(self, msg: Int32MultiArray) -> None: """ Callback method for hydrophone1 topic. @@ -108,7 +108,7 @@ def hydrophone1_callback(self, msg): """ self.hydrophone1_data = msg.data - def hydrophone2_callback(self, msg): + def hydrophone2_callback(self, msg: Int32MultiArray) -> None: """ Callback method for hydrophone2 topic. @@ -117,7 +117,7 @@ def hydrophone2_callback(self, msg): """ self.hydrophone2_data = msg.data - def hydrophone3_callback(self, msg): + def hydrophone3_callback(self, msg: Int32MultiArray) -> None: """ Callback method for hydrophone3 topic. @@ -126,7 +126,7 @@ def hydrophone3_callback(self, msg): """ self.hydrophone3_data = msg.data - def hydrophone4_callback(self, msg): + def hydrophone4_callback(self, msg: Int32MultiArray) -> None: """ Callback method for hydrophone4 topic. @@ -135,7 +135,7 @@ def hydrophone4_callback(self, msg): """ self.hydrophone4_data = msg.data - def hydrophone5_callback(self, msg): + def hydrophone5_callback(self, msg: Int32MultiArray) -> None: """ Callback method for hydrophone5 topic. @@ -144,7 +144,7 @@ def hydrophone5_callback(self, msg): """ self.hydrophone5_data = msg.data - def filter_response_callback(self, msg): + def filter_response_callback(self, msg: Int32MultiArray) -> None: """ Callback method for filter_response topic. @@ -153,7 +153,7 @@ def filter_response_callback(self, msg): """ self.filter_response_data = msg.data - def fft_callback(self, msg): + def fft_callback(self, msg: Int32MultiArray) -> None: """ Callback method for fft topic. @@ -162,7 +162,7 @@ def fft_callback(self, msg): """ self.fft_data = msg.data - def peaks_callback(self, msg): + def peaks_callback(self, msg: Int32MultiArray) -> None: """ Callback method for peaks topic. @@ -171,7 +171,7 @@ def peaks_callback(self, msg): """ self.peaks_data = msg.data - def tdoa_callback(self, msg): + def tdoa_callback(self, msg: Float32MultiArray) -> None: """ Callback method for time_difference_of_arrival topic. @@ -180,7 +180,7 @@ def tdoa_callback(self, msg): """ self.tdoa_data = msg.data - def position_callback(self, msg): + def position_callback(self, msg: Float32MultiArray) -> None: """ Callback method for position topic. @@ -190,7 +190,7 @@ def position_callback(self, msg): self.position_data = msg.data # The logger that logs all the data - def logger(self): + def logger(self) -> None: """ Logs all the data to a CSV file using the AcousticsDataRecordLib. @@ -210,7 +210,7 @@ def logger(self): ) -def main(): +def main() -> None: """ Main function to initialize and run the ROS2 node for acoustics data recording. 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 548b3e65a..085c82c38 100755 --- a/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py +++ b/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py @@ -5,7 +5,7 @@ from launch_ros.actions import Node -def generate_launch_description(): +def generate_launch_description() -> LaunchDescription: """ Generates a launch description for the acoustics_data_record node. 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 11799b990..30af95235 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -64,7 +64,7 @@ acousticsCSVFile = max(acousticsCSVFiles, key=os.path.getctime) -def convert_pandas_object_to_int_array(pandas_object): +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. @@ -80,7 +80,7 @@ def convert_pandas_object_to_int_array(pandas_object): return pandas_int_array -def convert_pandas_object_to_float_array(pandas_object): +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. @@ -96,7 +96,7 @@ def convert_pandas_object_to_float_array(pandas_object): return pandas_float_array -def get_acoustics_data(): +def get_acoustics_data() -> list: """ Retrieves and processes the latest acoustics data from a CSV file. @@ -218,7 +218,7 @@ def get_acoustics_data(): ] -def display_live_data(): +def display_live_data() -> None: """ Display live acoustics data by plotting hydrophone data, filter response, and FFT data. diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py index 264f8367b..dbdb00d6b 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py @@ -94,7 +94,7 @@ def data_publisher(self) -> None: self._position_publisher.publish(Float32MultiArray(data=TeensyCommunicationUDP.acoustics_data["LOCATION"])) -def main(args=None): +def main(args: list=None) -> None: """ Entry point for the acoustics interface node. diff --git a/acoustics/acoustics_interface/launch/acoustics_interface_launch.py b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py index 91f3da639..a283d3b22 100755 --- a/acoustics/acoustics_interface/launch/acoustics_interface_launch.py +++ b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py @@ -5,7 +5,7 @@ from launch_ros.actions import Node -def generate_launch_description(): +def generate_launch_description() -> LaunchDescription: """ Generates a launch description for the acoustics_interface node. diff --git a/auv_setup/launch/orca.launch.py b/auv_setup/launch/orca.launch.py index 34a88f02f..13a57acbe 100755 --- a/auv_setup/launch/orca.launch.py +++ b/auv_setup/launch/orca.launch.py @@ -6,7 +6,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource -def generate_launch_description(): +def generate_launch_description() -> LaunchDescription: """ Generates a launch description for the ORCA AUV setup. diff --git a/auv_setup/launch/topside.launch.py b/auv_setup/launch/topside.launch.py index 4058b79cf..c0073bf3d 100755 --- a/auv_setup/launch/topside.launch.py +++ b/auv_setup/launch/topside.launch.py @@ -7,7 +7,7 @@ from launch_ros.actions import Node -def generate_launch_description(): +def generate_launch_description() -> LaunchDescription: """ Generate the launch description for the topside AUV setup. diff --git a/mission/LCD/sources/ip_lib.py b/mission/LCD/sources/ip_lib.py index c16ea0538..5719249e1 100644 --- a/mission/LCD/sources/ip_lib.py +++ b/mission/LCD/sources/ip_lib.py @@ -4,10 +4,10 @@ class IPDriver: - def __init__(self): + def __init__(self) -> None: self.cmd = "hostname -I | cut -d' ' -f1" - def get_ip(self): + def get_ip(self) -> str: """ Executes a shell command to retrieve the IP address. diff --git a/mission/LCD/sources/lcd.py b/mission/LCD/sources/lcd.py index 32e268d91..83ae63dbc 100644 --- a/mission/LCD/sources/lcd.py +++ b/mission/LCD/sources/lcd.py @@ -20,7 +20,7 @@ # Formatting function for nices LCD screen layout -def format_line(value: str, unit: str): +def format_line(value: str, unit: str) -> str: """ Formats a string to fit within a 16-character display, appending a unit with spacing. diff --git a/mission/LCD/sources/lcd_lib.py b/mission/LCD/sources/lcd_lib.py index 6b62ae8cc..8274794cf 100644 --- a/mission/LCD/sources/lcd_lib.py +++ b/mission/LCD/sources/lcd_lib.py @@ -7,7 +7,7 @@ class LCDScreenDriver: - def __init__(self): + def __init__(self) -> None: # Initialize LCD Screen lcd_i2c_address = 0x27 @@ -23,7 +23,7 @@ def __init__(self): backlight_enabled=True, ) - def write_to_screen(self, line1: str = "", line2: str = ""): + def write_to_screen(self, line1: str = "", line2: str = "") -> None: """ Writes two lines of text to the LCD screen. @@ -47,7 +47,7 @@ def write_to_screen(self, line1: str = "", line2: str = ""): self._lcd.write_string(line1 + "\r\n") self._lcd.write_string(line2) - def fancy_animation(self, animation_speed=0.4): + 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. diff --git a/mission/LCD/sources/power_sense_module_lib.py b/mission/LCD/sources/power_sense_module_lib.py index 65ba6bd3a..e57195806 100755 --- a/mission/LCD/sources/power_sense_module_lib.py +++ b/mission/LCD/sources/power_sense_module_lib.py @@ -6,7 +6,7 @@ class PowerSenseModule: - def __init__(self): + def __init__(self) -> None: # Parameters # to read voltage and current from ADC on PDB through I2C self.i2c_adress = 0x69 @@ -33,7 +33,7 @@ def __init__(self): self.psm_to_battery_current_scale_factor = 37.8788 # A/V self.psm_to_battery_current_offset = 0.330 # V - def get_voltage(self): + def get_voltage(self) -> float: """ Retrieves the system voltage by reading and converting the channel voltage. @@ -53,7 +53,7 @@ def get_voltage(self): print(f"ERROR: Failed retrieving voltage from PSM: {error}") return 0.0 - def get_current(self): + def get_current(self) -> float: """ Retrieves the current system current by reading from the current channel, applying an offset, and scaling the result. diff --git a/mission/LCD/sources/pressure_sensor_lib.py b/mission/LCD/sources/pressure_sensor_lib.py index fbab595a7..8e89b6960 100755 --- a/mission/LCD/sources/pressure_sensor_lib.py +++ b/mission/LCD/sources/pressure_sensor_lib.py @@ -9,7 +9,7 @@ class PressureSensor: - def __init__(self): + def __init__(self) -> None: # Pressure Sensor Setup i2c_adress_mprls = 0x18 # Reads pressure from MPRLS Adafruit sensor self.channel_pressure = None @@ -29,7 +29,7 @@ def __init__(self): time.sleep(1) - def get_pressure(self): + def get_pressure(self) -> float: """ Retrieves the current pressure from the pressure sensor. diff --git a/mission/LCD/sources/temperature_sensor_lib.py b/mission/LCD/sources/temperature_sensor_lib.py index 077dd6118..6f5f2bb65 100755 --- a/mission/LCD/sources/temperature_sensor_lib.py +++ b/mission/LCD/sources/temperature_sensor_lib.py @@ -11,11 +11,11 @@ class TemperatureSensor: - def __init__(self): + def __init__(self) -> None: # Temperature Sensor Setup self.temperature_sensor_file_location = "/sys/class/thermal/thermal_zone0/temp" - def get_temperature(self): + def get_temperature(self) -> float: """ Reads the internal temperature from the specified sensor file location. diff --git a/mission/blackbox/blackbox/blackbox_log_data.py b/mission/blackbox/blackbox/blackbox_log_data.py index d8042b0b2..f8671b5f3 100755 --- a/mission/blackbox/blackbox/blackbox_log_data.py +++ b/mission/blackbox/blackbox/blackbox_log_data.py @@ -9,7 +9,7 @@ class BlackBoxLogData: - def __init__(self, ros2_package_directory=""): + def __init__(self, ros2_package_directory: str="") -> None: # Global variables for .csv file manipulation ---------- # Get the path for the directory where we will store our data self.blackbox_data_directory = ros2_package_directory + "blackbox_data/" @@ -53,7 +53,7 @@ def __init__(self, ros2_package_directory=""): writer.writerow(self.csv_headers) # Methods for inside use of the class ---------- - def manage_csv_files(self, max_file_age_in_days=7, max_size_kb=3_000_000): + 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. @@ -143,27 +143,27 @@ def manage_csv_files(self, max_file_age_in_days=7, max_size_kb=3_000_000): # Methods for external uses ---------- def log_data_to_csv_file( self, - psm_current=0.0, - psm_voltage=0.0, - pressure_internal=0.0, - temperature_ambient=0.0, - thruster_forces_1=0.0, - thruster_forces_2=0.0, - thruster_forces_3=0.0, - thruster_forces_4=0.0, - thruster_forces_5=0.0, - thruster_forces_6=0.0, - thruster_forces_7=0.0, - thruster_forces_8=0.0, - pwm_1=0, - pwm_2=0, - pwm_3=0, - pwm_4=0, - pwm_5=0, - pwm_6=0, - pwm_7=0, - pwm_8=0, - ): + psm_current: float = 0.0, + psm_voltage: float = 0.0, + pressure_internal: float = 0.0, + temperature_ambient: float = 0.0, + thruster_forces_1: float = 0.0, + thruster_forces_2: float = 0.0, + thruster_forces_3: float = 0.0, + thruster_forces_4: float = 0.0, + thruster_forces_5: float = 0.0, + thruster_forces_6: float = 0.0, + thruster_forces_7: float = 0.0, + thruster_forces_8: float = 0.0, + pwm_1: int = 0, + pwm_2: int = 0, + pwm_3: int = 0, + pwm_4: int = 0, + pwm_5: int = 0, + pwm_6: int = 0, + pwm_7: int = 0, + pwm_8: int = 0, + ) -> None: """ Logs the provided data to a CSV file. Parameters: diff --git a/mission/blackbox/blackbox/blackbox_node.py b/mission/blackbox/blackbox/blackbox_node.py index 73c6f13fd..f5d117541 100755 --- a/mission/blackbox/blackbox/blackbox_node.py +++ b/mission/blackbox/blackbox/blackbox_node.py @@ -16,7 +16,7 @@ class BlackBoxNode(Node): - def __init__(self): + def __init__(self) -> None: # Start the ROS2 Node ---------- super().__init__("blackbox_node") @@ -66,7 +66,7 @@ def __init__(self): ) # Callback Methods ---------- - def psm_current_callback(self, msg): + def psm_current_callback(self, msg: Float32) -> None: """ Callback function for the power sense module (PSM) current topic. @@ -79,7 +79,7 @@ def psm_current_callback(self, msg): """ self.psm_current_data = msg.data - def psm_voltage_callback(self, msg): + def psm_voltage_callback(self, msg: Float32) -> None: """ Callback function for the power sense module (PSM) voltage topic. @@ -92,7 +92,7 @@ def psm_voltage_callback(self, msg): """ self.psm_voltage_data = msg.data - def pressure_callback(self, msg): + def pressure_callback(self, msg: Float32) -> None: """ Callback function for the internal pressure topic. @@ -105,7 +105,7 @@ def pressure_callback(self, msg): """ self.pressure_data = msg.data - def temperature_callback(self, msg): + def temperature_callback(self, msg: Float32) -> None: """ Callback function for the ambient temperature topic. @@ -118,7 +118,7 @@ def temperature_callback(self, msg): """ self.temperature_data = msg.data - def thruster_forces_callback(self, msg): + def thruster_forces_callback(self, msg: ThrusterForces) -> None: """ Callback function for the thruster forces topic. @@ -131,7 +131,7 @@ def thruster_forces_callback(self, msg): """ self.thruster_forces_data = msg.thrust - def pwm_callback(self, msg): + def pwm_callback(self, msg: Int16MultiArray) -> None: """ Callback function for the PWM signals topic. @@ -143,7 +143,7 @@ def pwm_callback(self, msg): """ self.pwm_data = msg.data - def logger(self): + def logger(self) -> None: """ Logs various sensor and actuator data to a CSV file. @@ -185,7 +185,7 @@ def logger(self): ) -def main(args=None): +def main(args: list=None) -> None: """ Entry point for the blackbox_node. diff --git a/mission/blackbox/launch/blackbox.launch.py b/mission/blackbox/launch/blackbox.launch.py index 0b0af38ac..5016f49b8 100644 --- a/mission/blackbox/launch/blackbox.launch.py +++ b/mission/blackbox/launch/blackbox.launch.py @@ -5,7 +5,7 @@ from launch_ros.actions import Node -def generate_launch_description(): +def generate_launch_description() -> LaunchDescription: """ Generates a launch description for the blackbox node. 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 651e46acb..5f737210e 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 @@ -6,7 +6,7 @@ class PowerSenseModule: - def __init__(self): + def __init__(self) -> None: # Parameters # to read voltage and current from ADC on PDB through I2C self.i2c_adress = 0x69 @@ -33,7 +33,7 @@ def __init__(self): self.psm_to_battery_current_scale_factor = 37.8788 # A/V self.psm_to_battery_current_offset = 0.330 # V - def get_voltage(self): + def get_voltage(self) -> float: """ Retrieves the voltage measurement from the Power Sense Module (PSM). @@ -54,7 +54,7 @@ def get_voltage(self): print(f"ERROR: Failed retrieving voltage from PSM: {error}") return 0.0 - def get_current(self): + def get_current(self) -> float: """ Retrieves the current measurement from the Power Sense Module (PSM). 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 976f2baf6..41bd9bb4f 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 @@ -9,7 +9,7 @@ class PowerSenseModulePublisher(Node): - def __init__(self): + def __init__(self) -> None: # Node setup ---------- super().__init__("power_sense_module_publisher") self.psm = internal_status_auv.power_sense_module_lib.PowerSenseModule() @@ -46,7 +46,7 @@ def __init__(self): # Debugging ---------- self.get_logger().info('"power_sense_module_publisher" has been started') - def read_timer_callback(self): + def read_timer_callback(self) -> None: """ Callback function triggered by the read timer. @@ -67,7 +67,7 @@ def read_timer_callback(self): 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): + def warning_timer_callback(self) -> None: """ Callback function triggered by the warning timer. @@ -80,7 +80,7 @@ def warning_timer_callback(self): self.logger.fatal(f"WARNING: Battery Voltage to HIGH at {self.voltage} V") -def main(args=None): +def main(args: list=None) -> None: """ Main function to initialize and spin the ROS2 node. 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 6b209b655..cbe6f8391 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 @@ -9,7 +9,7 @@ class PressureSensor: - def __init__(self): + def __init__(self) -> None: # Pressure Sensor Setup i2c_adress_mprls = 0x18 # Reads pressure from MPRLS Adafruit sensor self.channel_pressure = None @@ -29,7 +29,7 @@ def __init__(self): time.sleep(1) - def get_pressure(self): + def get_pressure(self) -> float: """ Retrieves the current pressure reading from the sensor. 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 90414efb4..4a7af29aa 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 @@ -9,7 +9,7 @@ class PressurePublisher(Node): - def __init__(self): + def __init__(self) -> None: # Pressure sensor setup ---------- self.pressure = internal_status_auv.pressure_sensor_lib.PressureSensor() @@ -41,7 +41,7 @@ def __init__(self): # Debugging ---------- self.get_logger().info('"pressure_sensor_publisher" has been started') - def timer_callback(self): + def timer_callback(self) -> None: """ Callback function triggered by the main timer. @@ -56,7 +56,7 @@ def timer_callback(self): pressure_msg.data = self.pressure self.publisher_pressure.publish(pressure_msg) - def warning_timer_callback(self): + def warning_timer_callback(self) -> None: """ Callback function triggered by the warning timer. @@ -67,7 +67,7 @@ def warning_timer_callback(self): self.logger.fatal(f"WARNING: Internal pressure to HIGH: {self.pressure} hPa! Drone might be LEAKING!") -def main(args=None): +def main(args: list=None) -> None: """ Main function to initialize and spin the ROS2 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 35c3a45d5..35cd857f1 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 @@ -11,11 +11,11 @@ class TemperatureSensor: - def __init__(self): + def __init__(self) -> None: # Temperature Sensor Setup self.temperature_sensor_file_location = "/sys/class/thermal/thermal_zone0/temp" - def get_temperature(self): + def get_temperature(self) -> float: """ Gets the current temperature from the internal computer's sensor. 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 5500c4421..6c48ca049 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 @@ -9,7 +9,7 @@ class TemperaturePublisher(Node): - def __init__(self): + def __init__(self) -> None: # Pressure sensor setup ---------- self.temperature = internal_status_auv.temperature_sensor_lib.TemperatureSensor() @@ -41,7 +41,7 @@ def __init__(self): # Debugging ---------- self.get_logger().info('"temperature_sensor_publisher" has been started') - def timer_callback(self): + def timer_callback(self) -> None: """ Callback function triggered by the main timer. @@ -56,7 +56,7 @@ def timer_callback(self): temperature_msg.data = self.temperature self.publisher_temperature.publish(temperature_msg) - def warning_timer_callback(self): + def warning_timer_callback(self) -> None: """ Callback function triggered by the warning timer. @@ -67,7 +67,7 @@ def warning_timer_callback(self): self.logger.fatal(f"WARNING: Temperature inside the Drone to HIGH: {self.temperature} *C! Drone might be overheating!") -def main(args=None): +def main(args: list=None) -> None: """ Main function to initialize and spin the ROS2 node. 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 b9e50229f..0b491a903 100644 --- a/mission/internal_status_auv/launch/internal_status_auv.launch.py +++ b/mission/internal_status_auv/launch/internal_status_auv.launch.py @@ -5,7 +5,7 @@ from launch_ros.actions import Node -def generate_launch_description(): +def generate_launch_description() -> LaunchDescription: """ Generates a LaunchDescription object that defines 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 c58dc70da..dd4aa3bf0 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 @@ -73,7 +73,7 @@ class WirelessXboxSeriesX: class JoystickInterface(Node): - def __init__(self): + def __init__(self) -> None: super().__init__("joystick_interface_node") self.get_logger().info( "Joystick interface is up and running. \n When the XBOX controller is connected, press the killswitch button once to enter XBOX mode." @@ -149,14 +149,14 @@ def create_wrench_message( wrench_msg.torque.z = yaw return wrench_msg - def transition_to_xbox_mode(self): + def transition_to_xbox_mode(self) -> None: """ 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): + def transition_to_autonomous_mode(self) -> None: """ Publishes a zero force wrench message and signals that the system is turning on autonomous mode. """ @@ -281,7 +281,7 @@ def joystick_cb(self, msg: Joy) -> Wrench: return wrench_msg -def main(): +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 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 db99b02ef..1f0ecbc6e 100755 --- a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py +++ b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py @@ -5,7 +5,7 @@ from launch_ros.actions import Node -def generate_launch_description(): +def generate_launch_description() -> LaunchDescription: """ Generates a launch description for the joystick_interface_auv node. 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 f0c504ac7..707e9e28f 100644 --- a/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py +++ b/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py @@ -5,7 +5,7 @@ class TestJoystickInterface: # test that the wrench msg is created successfully - def test_wrench_msg(self): + def test_wrench_msg(self) -> None: """ Test the creation of a Wrench message using the JoystickInterface. @@ -33,7 +33,7 @@ def test_wrench_msg(self): rclpy.shutdown() # Test that the callback function will be able to interpret the joy msg - def test_input_from_controller_into_wrench_msg(self): + def test_input_from_controller_into_wrench_msg(self) -> None: """ Test the conversion of joystick input to wrench message. @@ -69,7 +69,7 @@ def test_input_from_controller_into_wrench_msg(self): rclpy.shutdown() # When the killswitch button is activated in the buttons list, it should output a wrench msg with only zeros - def test_killswitch_button(self): + def test_killswitch_button(self) -> None: """ Test the killswitch button functionality of the JoystickInterface. @@ -105,7 +105,7 @@ def test_killswitch_button(self): rclpy.shutdown() # When we move into XBOX mode it should still be able to return this wrench message - def test_moving_in_of_xbox_mode(self): + def test_moving_in_of_xbox_mode(self) -> None: """ Test the joystick callback function in XBOX mode. 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 b7383ded5..decab0783 100644 --- a/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py +++ b/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py @@ -5,7 +5,7 @@ from launch_ros.actions import Node -def generate_launch_description(): +def generate_launch_description() -> LaunchDescription: """ Generates a launch description for the thrust_allocator_auv_node. 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 47805d804..ebb5a7aa2 100644 --- a/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py +++ b/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py @@ -5,7 +5,7 @@ from launch_ros.actions import Node -def generate_launch_description(): +def generate_launch_description() -> LaunchDescription: """ Generates a launch description for the 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 5a6537aaa..6511fbe5f 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,5 +1,7 @@ #!/usr/bin/env python3 # Import libraries +from typing import List + import numpy import pandas import smbus2 @@ -8,16 +10,16 @@ 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: 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: @@ -51,7 +53,7 @@ def __init__( # 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): + 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: @@ -85,7 +87,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: list) -> None: i2c_data_array = [] # Divide data into bytes as I2C only sends bytes @@ -100,7 +102,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) -> list: """ Takes in Array of forces in Newtosn [N] takes 8 floats in form of: 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 99a3c8245..22b1e3c0f 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 @@ -13,7 +13,7 @@ class ThrusterInterfaceAUVNode(Node): - def __init__(self): + def __init__(self) -> None: # Initialize and name the node process running super().__init__("thruster_interface_auv_node") @@ -64,11 +64,11 @@ 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: ThrusterForces) -> 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 debugging feature to see if everything is alright with the PWM signal thruster_pwm_array = self.thruster_driver.drive_thrusters(self.thruster_forces_array) @@ -78,7 +78,7 @@ def _timer_callback(self): self.thruster_pwm_publisher.publish(pwm_message) -def main(args=None): +def main(args: list=None) -> None: """ Entry point for the thruster interface AUV node. From 6dc658a6c5dd30a80e88b070d12552618101492a Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Wed, 25 Sep 2024 13:03:38 +0200 Subject: [PATCH 44/51] ci: add mypy.ini for type checking configuration --- mypy.ini | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 mypy.ini diff --git a/mypy.ini b/mypy.ini new file mode 100644 index 000000000..1a2b59ed2 --- /dev/null +++ b/mypy.ini @@ -0,0 +1,30 @@ +# 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 From fd50092d982a560bc866382b7dfc1fc1d95e0386 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Wed, 25 Sep 2024 13:06:06 +0200 Subject: [PATCH 45/51] ci: add mypy type checking workflow --- .github/workflows/mypy.yaml | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 .github/workflows/mypy.yaml diff --git a/.github/workflows/mypy.yaml b/.github/workflows/mypy.yaml new file mode 100644 index 000000000..e8c633a31 --- /dev/null +++ b/.github/workflows/mypy.yaml @@ -0,0 +1,22 @@ +name: mypy + +on: [pull_request] + +jobs: + mypy: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v4 + with: + ref: ${{ github.head_ref }} + + - name: Install mypy + run: | + sudo apt-get update + sudo apt-get install -y python3-pip + pip3 install mypy + + - name: Run mypy type checking + run: | + mypy . --explicit-package-bases From 352e8ab7511011a60c315ebbe6d1a032c751e0cd Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Wed, 25 Sep 2024 13:06:49 +0200 Subject: [PATCH 46/51] refactor: format code with black --- .../acoustics_interface/acoustics_interface_node.py | 2 +- mission/LCD/sources/lcd_lib.py | 2 +- mission/blackbox/blackbox/blackbox_log_data.py | 2 +- mission/blackbox/blackbox/blackbox_node.py | 2 +- .../internal_status_auv/power_sense_module_node.py | 2 +- .../internal_status_auv/pressure_sensor_node.py | 2 +- .../internal_status_auv/temperature_sensor_node.py | 2 +- .../thruster_interface_auv/thruster_interface_auv_node.py | 2 +- 8 files changed, 8 insertions(+), 8 deletions(-) diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py index dbdb00d6b..4c2e4f60d 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py @@ -94,7 +94,7 @@ def data_publisher(self) -> None: self._position_publisher.publish(Float32MultiArray(data=TeensyCommunicationUDP.acoustics_data["LOCATION"])) -def main(args: list=None) -> None: +def main(args: list = None) -> None: """ Entry point for the acoustics interface node. diff --git a/mission/LCD/sources/lcd_lib.py b/mission/LCD/sources/lcd_lib.py index 8274794cf..f77484a5c 100644 --- a/mission/LCD/sources/lcd_lib.py +++ b/mission/LCD/sources/lcd_lib.py @@ -47,7 +47,7 @@ def write_to_screen(self, line1: str = "", line2: str = "") -> None: self._lcd.write_string(line1 + "\r\n") self._lcd.write_string(line2) - def fancy_animation(self, animation_speed: float=0.4) -> None: + 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. diff --git a/mission/blackbox/blackbox/blackbox_log_data.py b/mission/blackbox/blackbox/blackbox_log_data.py index f8671b5f3..0344897c3 100755 --- a/mission/blackbox/blackbox/blackbox_log_data.py +++ b/mission/blackbox/blackbox/blackbox_log_data.py @@ -9,7 +9,7 @@ class BlackBoxLogData: - def __init__(self, ros2_package_directory: str="") -> None: + def __init__(self, ros2_package_directory: str = "") -> None: # Global variables for .csv file manipulation ---------- # Get the path for the directory where we will store our data self.blackbox_data_directory = ros2_package_directory + "blackbox_data/" diff --git a/mission/blackbox/blackbox/blackbox_node.py b/mission/blackbox/blackbox/blackbox_node.py index f5d117541..b82f17233 100755 --- a/mission/blackbox/blackbox/blackbox_node.py +++ b/mission/blackbox/blackbox/blackbox_node.py @@ -185,7 +185,7 @@ def logger(self) -> None: ) -def main(args: list=None) -> None: +def main(args: list = None) -> None: """ Entry point for the blackbox_node. 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 41bd9bb4f..2e94de425 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 @@ -80,7 +80,7 @@ def warning_timer_callback(self) -> None: self.logger.fatal(f"WARNING: Battery Voltage to HIGH at {self.voltage} V") -def main(args: list=None) -> None: +def main(args: list = None) -> None: """ Main function to initialize and spin the ROS2 node. 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 4a7af29aa..74d1b04b8 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 @@ -67,7 +67,7 @@ def warning_timer_callback(self) -> None: self.logger.fatal(f"WARNING: Internal pressure to HIGH: {self.pressure} hPa! Drone might be LEAKING!") -def main(args: list=None) -> None: +def main(args: list = None) -> None: """ Main function to initialize and spin the ROS2 node. 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 6c48ca049..f39aa563c 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 @@ -67,7 +67,7 @@ def warning_timer_callback(self) -> None: self.logger.fatal(f"WARNING: Temperature inside the Drone to HIGH: {self.temperature} *C! Drone might be overheating!") -def main(args: list=None) -> None: +def main(args: list = None) -> None: """ Main function to initialize and spin the ROS2 node. 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 22b1e3c0f..6751f82b1 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 @@ -78,7 +78,7 @@ def _timer_callback(self) -> None: self.thruster_pwm_publisher.publish(pwm_message) -def main(args: list=None) -> None: +def main(args: list = None) -> None: """ Entry point for the thruster interface AUV node. From 4a3e1c2398d5e17cf376d289dbebd31171fd31a6 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Wed, 25 Sep 2024 17:56:15 +0200 Subject: [PATCH 47/51] refactor: format all yaml files using prettier --- .github/workflows/clang-formatter.yaml | 30 ++-- .github/workflows/codespell.yaml | 4 +- .github/workflows/docker-publish.yml | 20 +-- .github/workflows/python-format-lint.yaml | 31 ++-- auv_setup/config/robots/beluga.yaml | 185 ++++++++++++++-------- auv_setup/config/robots/maelstrom.yaml | 21 +-- auv_setup/config/robots/manta.yaml | 117 ++++++++++---- auv_setup/config/robots/manta_enu.yaml | 145 ++++++++++++----- auv_setup/config/robots/manta_rov.yaml | 117 ++++++++++---- auv_setup/config/robots/orca.yaml | 112 +++++++++---- auv_setup/config/robots/terrapin.yaml | 95 ++++++++--- 11 files changed, 608 insertions(+), 269 deletions(-) diff --git a/.github/workflows/clang-formatter.yaml b/.github/workflows/clang-formatter.yaml index 535afdac3..5a3b02b2c 100644 --- a/.github/workflows/clang-formatter.yaml +++ b/.github/workflows/clang-formatter.yaml @@ -7,18 +7,18 @@ jobs: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 - - uses: DoozyX/clang-format-lint-action@v0.18.1 - with: - source: '.' - exclude: './lib' - extensions: 'h,cpp,c' - clangFormatVersion: 18 - inplace: True - - uses: EndBug/add-and-commit@v9 - with: - author_name: Clang Robot - author_email: robot@example.com - message: 'Committing clang-format changes' - env: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} \ No newline at end of file + - uses: actions/checkout@v2 + - uses: DoozyX/clang-format-lint-action@v0.18.1 + with: + source: "." + exclude: "./lib" + extensions: "h,cpp,c" + clangFormatVersion: 18 + inplace: True + - uses: EndBug/add-and-commit@v9 + with: + author_name: Clang Robot + author_email: robot@example.com + message: "Committing clang-format changes" + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/codespell.yaml b/.github/workflows/codespell.yaml index ee92c29a6..64f64e6bb 100644 --- a/.github/workflows/codespell.yaml +++ b/.github/workflows/codespell.yaml @@ -25,6 +25,6 @@ jobs: with: author_name: Codespell Robot author_email: codespell-robot@example.com - message: 'Committing codespell fixes' + message: "Committing codespell fixes" env: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} \ No newline at end of file + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/docker-publish.yml b/.github/workflows/docker-publish.yml index 87d9085ca..0bb2cce3b 100644 --- a/.github/workflows/docker-publish.yml +++ b/.github/workflows/docker-publish.yml @@ -2,7 +2,7 @@ name: Publish Docker image to ghcr on: push: - branches: [ development ] + branches: [development] env: REGISTRY: ghcr.io @@ -18,32 +18,32 @@ jobs: 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' + 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 + + # 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' + 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 diff --git a/.github/workflows/python-format-lint.yaml b/.github/workflows/python-format-lint.yaml index fc283e62a..3c7283fd3 100644 --- a/.github/workflows/python-format-lint.yaml +++ b/.github/workflows/python-format-lint.yaml @@ -19,13 +19,13 @@ jobs: with: author_name: Isort Robot author_email: isort-robot@example.com - message: 'Committing isort changes' + message: "Committing isort changes" env: GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} black: runs-on: ubuntu-latest - needs: isort + needs: isort steps: - uses: actions/checkout@v4 @@ -43,7 +43,7 @@ jobs: with: author_name: Black Robot author_email: black-robot@example.com - message: 'Committing black-format changes' + message: "Committing black-format changes" env: GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} @@ -54,16 +54,15 @@ jobs: matrix: python-version: ["3.9", "3.10", "3.11"] steps: - - uses: actions/checkout@v4 - - name: Set up Python ${{ matrix.python-version }} - uses: actions/setup-python@v3 - with: - python-version: ${{ matrix.python-version }} - - name: Install dependencies - run: | - python -m pip install --upgrade pip - pip install pylint - - name: Analysing the code with pylint - run: | - pylint $(git ls-files '*.py') - + - uses: actions/checkout@v4 + - name: Set up Python ${{ matrix.python-version }} + uses: actions/setup-python@v3 + with: + python-version: ${{ matrix.python-version }} + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install pylint + - name: Analysing the code with pylint + run: | + pylint $(git ls-files '*.py') diff --git a/auv_setup/config/robots/beluga.yaml b/auv_setup/config/robots/beluga.yaml index fe078e15a..245580e69 100644 --- a/auv_setup/config/robots/beluga.yaml +++ b/auv_setup/config/robots/beluga.yaml @@ -1,6 +1,6 @@ # This file defines parameters specific to Beluga # -# When looking at the AUV from above, the thruster placement is: +# When looking at the AUV from above, the thruster placement is: # # front # |======| @@ -15,42 +15,95 @@ # physical: - weight: 35.0 #24.00 # kg + weight: 35.0 #24.00 # kg buoyancy: 36.6 #37.522 #25 # kg (pho*V) - center_of_mass: [0.0, 0.0, -0.08] # m (x,y,z) - center_of_buoyancy: [0.0, 0.0, 0.5] # m (x,y,z) - + center_of_mass: [0.0, 0.0, -0.08] # m (x,y,z) + center_of_buoyancy: [0.0, 0.0, 0.5] # m (x,y,z) propulsion: dofs: num: 6 which: surge: true - sway: true + sway: true heave: true - roll: true + roll: true pitch: true - yaw: true + yaw: true thrusters: thrusters_operational_voltage_range: [14.2, 17.0] # Volts # If less or more than set Voltage -> thrusters shut down num: 8 - configuration_matrix: - - [[ 0.70711, 0.00000, 0.00000, 0.70711, 0.70711, 0.00000, 0.00000, 0.70711], # Surge - [0.70711, 0.00000, 0.00000, -0.70711, 0.70711, 0.00000, 0.00000, -0.70711], # Sway - [ 0.00000, 1.00000, 1.00000, 0.00000, 0.00000, 1.00000, 1.00000, 0.00000], # Heave + configuration_matrix: [ + [ + 0.70711, + 0.00000, + 0.00000, + 0.70711, + 0.70711, + 0.00000, + 0.00000, + 0.70711, + ], # Surge + [ + 0.70711, + 0.00000, + 0.00000, + -0.70711, + 0.70711, + 0.00000, + 0.00000, + -0.70711, + ], # Sway + [ + 0.00000, + 1.00000, + 1.00000, + 0.00000, + 0.00000, + 1.00000, + 1.00000, + 0.00000, + ], # Heave - [ 0.00000, -0.20600, -0.20600, 0.00000, 0.00000, 0.20600, 0.20600, 0.00000 ], # Roll - [ 0.00000, -0.09600, 0.09600, 0.00000, 0.00000, 0.09600, -0.09600, 0.00000 ], # Pitch - [0.30000, 0.00000, 0.00000, 0.30000, -0.30000, 0.00000, 0.00000, -0.30000 ]] # Yaw + [ + 0.00000, + -0.20600, + -0.20600, + 0.00000, + 0.00000, + 0.20600, + 0.20600, + 0.00000, + ], # Roll + [ + 0.00000, + -0.09600, + 0.09600, + 0.00000, + 0.00000, + 0.09600, + -0.09600, + 0.00000, + ], # Pitch + [ + 0.30000, + 0.00000, + 0.00000, + 0.30000, + -0.30000, + 0.00000, + 0.00000, + -0.30000, + ], + ] # Yaw rate_of_change: max: 1 # Maximum rate of change in newton per second for a thruster - map: [ 4, 6, 7, 0, 3, 1, 2, 5 ] # [ 0, 1, 2, 3, 4, 5, 6, 7 ] - direction: [ 1, -1, 1, 1, -1, 1, 1, -1 ] - offset: [ 80, 80, 80, 80, 80, 80, 80, 80 ] # Offset IN PWM -400 to 400 # Offset moves where the thrust is at rest - thrust_range: [-0.7, 0.7] # range in percentage -1.0 to 1.0 # NOTE!: thrust_range moves with the offset, if the offset is too big e.g., ± ~400 the thrust range will go out of bounds + map: [4, 6, 7, 0, 3, 1, 2, 5] # [ 0, 1, 2, 3, 4, 5, 6, 7 ] + direction: [1, -1, 1, 1, -1, 1, 1, -1] + offset: [80, 80, 80, 80, 80, 80, 80, 80] # Offset IN PWM -400 to 400 # Offset moves where the thrust is at rest + thrust_range: [-0.7, 0.7] # range in percentage -1.0 to 1.0 # NOTE!: thrust_range moves with the offset, if the offset is too big e.g., ± ~400 the thrust range will go out of bounds guidance: dp: @@ -62,19 +115,18 @@ guidance: delta: 0.7 odom_topic: "/odometry/filtered" action_server: "/guidance_interface/los_server" - vel: + vel: rate: 20 joy: thrust_topic: "/thrust/joy" - controllers: dp: thrust_topic: "/thrust/dp" odometry_topic: "/odometry/filtered" - velocity_gain: 1.0 # lower (1.0) is good when using slightly noisy state estimates - position_gain: 35.0 - attitude_gain: 7.5 + velocity_gain: 1.0 # lower (1.0) is good when using slightly noisy state estimates + position_gain: 35.0 + attitude_gain: 7.5 integral_gain: 0.065 los_controller: # Note: Not loaded by the backstepping/pid controllers yet! Make sure to do this after tuning with dyn. reconf. PID: @@ -83,7 +135,7 @@ controllers: d: 3.5 sat: 40.0 backstepping: - c: 0.5 + c: 0.5 k1: 25.0 k2: 10.0 k3: 10.0 @@ -97,20 +149,20 @@ controllers: I_gains: [1, 1, 2.5, 1.5, 0.015, 0.015] D_gains: [0.01, 0.01, 0.01, 0.12, 0.03, 0.03] F_gains: [50, 75, 75, 50, 10, 10] - integral_windup_limit: 10 # in newton - setpoint_range: 0.5 # max distance from current point to setpoint - max_output_ramp_rate: 1 # in newton per cycle + integral_windup_limit: 10 # in newton + setpoint_range: 0.5 # max distance from current point to setpoint + max_output_ramp_rate: 1 # in newton per cycle vtf: odometry_topic: "/odometry/filtered" sphere_of_acceptence: 0.2 control_input_saturation_limits: [-10.5, 10.5] - control_forces_weights: [0.1, 0.1, 0.1, 10, 10, 1] # Weights for what DOF is prioritized when allocation control forces. Leave - control_bandwidth: [0.6, 1, 0.6, 0.6, 0.6, 0.6] # Tune the PID control bandwidths for [surge, sway, heave, roll, pitch, yaw]. NB! roll and pitch are redundant + control_forces_weights: [0.1, 0.1, 0.1, 10, 10, 1] # Weights for what DOF is prioritized when allocation control forces. Leave + control_bandwidth: [0.6, 1, 0.6, 0.6, 0.6, 0.6] # Tune the PID control bandwidths for [surge, sway, heave, roll, pitch, yaw]. NB! roll and pitch are redundant relative_damping_ratio: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # Leave absolute_relative_velocity_limit: 0.5 # Leave - reference_model_control_bandwidth_gain: 0.5 # <= 1 # Leave + reference_model_control_bandwidth_gain: 0.5 # <= 1 # Leave reference_model_control_input_saturation_limit_gain: 0.8 # <= 1 Leave - virtual_target_controller_bandwidths: [1, 1, 0.75, 1, 1, 1.5] # Tune the path following precision for [surge, sway, heave, roll, pitch, yaw] NB! roll and pitch are redundant. Higher values gives better precision + virtual_target_controller_bandwidths: [1, 1, 0.75, 1, 1, 1.5] # Tune the path following precision for [surge, sway, heave, roll, pitch, yaw] NB! roll and pitch are redundant. Higher values gives better precision virtual_target_along_track_speed_saturation_limits: [-0.2, 0.2] # <= 1 Speed saturation limits for the virtual target along the path virtual_target_control_input_saturation_limit_gain: 0.8 # <= 1 Leave publish_rate: 20 # The node frequency @@ -119,23 +171,27 @@ controllers: first_order_time_constant: 0.2 rotor_constant: 0.00000364 positions: # w.r.t. ENU - [[ 0.238, -0.220, 0.065], - [ 0.120, -0.220, 0.065], - [ -0.120, -0.220, 0.065], - [ -0.240, -0.220, 0.065], - [ -0.240, 0.220, 0.065], - [ -0.120, 0.220, 0.065], - [ 0.120, 0.220, 0.065], - [ 0.238, 0.220, 0.065]] + [ + [0.238, -0.220, 0.065], + [0.120, -0.220, 0.065], + [-0.120, -0.220, 0.065], + [-0.240, -0.220, 0.065], + [-0.240, 0.220, 0.065], + [-0.120, 0.220, 0.065], + [0.120, 0.220, 0.065], + [0.238, 0.220, 0.065], + ] orientations: # w.r.t. ENU - [[ 0, 0, -2.356], - [ 0, -1.571, 0], - [ 0, -1.571, 0], - [ 0, 0, -0.785], - [ 0, 0, 0.785], - [ 0, -1.571, 0], - [ 0, -1.571, 0], - [ 0, 0, 2.356]] + [ + [0, 0, -2.356], + [0, -1.571, 0], + [0, -1.571, 0], + [0, 0, -0.785], + [0, 0, 0.785], + [0, -1.571, 0], + [0, -1.571, 0], + [0, 0, 2.356], + ] model_parameters: mass: 25.4 # kg @@ -143,16 +199,14 @@ controllers: center_of_mass: [0, 0, 0] # w.r.t. ENU center_of_buoyancy: [0, 0, 0.026] # w.r.t. ENU inertia: # Inertia about center of mass - [[ 0.5, 0, 0], - [ 0, 1.1, 0], - [ 0, 0, 1.2]] + [[0.5, 0, 0], [0, 1.1, 0], [0, 0, 1.2]] water_density: 997 M_A: [20, 40, 50, 0.5, 2, 2] # Added mass D: [-20, -40, -50, -3, -10, -12] # Linear damping thrust: thrust_topic: "/thrust/desired_forces" - + joystick: scaling: surge: 60 @@ -162,7 +216,6 @@ joystick: pitch: -30 yaw: 20 - fsm: landmark_topic: "/fsm/state" time_to_launch: 1 # seconds @@ -175,7 +228,7 @@ fsm: battery: thresholds: - warning: 14.5 # Volts + warning: 14.5 # Volts critical: 13.5 # Volts system: interval: 0.05 # seconds @@ -184,10 +237,10 @@ battery: interval: 10 path: "/sys/bus/i2c/drivers/ina3221x/1-0040/iio:device0/in_voltage0_input" # Path to monitor logging: - interval: 10 # Seconds + interval: 10 # Seconds pressure: - thresholds: + thresholds: critical: 1013.25 # hPa system: interval: 0.5 # seconds @@ -196,8 +249,8 @@ pressure: temperature: logging: - interval: 10 # Seconds - + interval: 10 # Seconds + i2c: pca9685: address: 0x40 @@ -209,7 +262,7 @@ i2c: pca9685: pwm: - bits_per_period: 4095 + bits_per_period: 4095 frequency: 50.0 frequency_measured: 50 # 51.6 is the former value, but we dont know why @@ -229,12 +282,12 @@ ping360_node: speedOfSound: 1500 queueSize: 1 threshold: 100 - enableImageTopic: True - enableScanTopic: True - enableDataTopic: True - maxAngle: 400 - minAngle: 0 - oscillate: False + enableImageTopic: True + enableScanTopic: True + enableDataTopic: True + maxAngle: 400 + minAngle: 0 + oscillate: False torpedo: gpio_pin: 15 diff --git a/auv_setup/config/robots/maelstrom.yaml b/auv_setup/config/robots/maelstrom.yaml index 2fc127fd6..6ba64171f 100644 --- a/auv_setup/config/robots/maelstrom.yaml +++ b/auv_setup/config/robots/maelstrom.yaml @@ -23,26 +23,27 @@ physical: mass_kg: 15 displacement_m3: 0.015 center_of_mass: [0, 0, -0.15] - center_of_buoyancy: [0, 0, 0.10] + center_of_buoyancy: [0, 0, 0.10] propulsion: dofs: num: 5 which: surge: true - sway: true + sway: true heave: true - roll: false + roll: false pitch: true - yaw: true + yaw: true thrusters: num: 6 - configuration_matrix: - [[ 0.7071, 0.7071, -0.7071, -0.7071, 0.0, 0.0], # Surge - [ 0.7071, -0.7071, -0.7071, 0.7071, 0.0, 0.0], # Sway - [ 0.0, 0.0, 0.0, 0.0, 1.0, 1.0], # Heave - [-0.0467, -0.0467, 0.0467, 0.0467, -0.1600, 0.1600], # Pitch - [ 0.2143, -0.2143, 0.2143, -0.2143, 0.0, 0.0]] # Yaw + configuration_matrix: [ + [0.7071, 0.7071, -0.7071, -0.7071, 0.0, 0.0], # Surge + [0.7071, -0.7071, -0.7071, 0.7071, 0.0, 0.0], # Sway + [0.0, 0.0, 0.0, 0.0, 1.0, 1.0], # Heave + [-0.0467, -0.0467, 0.0467, 0.0467, -0.1600, 0.1600], # Pitch + [0.2143, -0.2143, 0.2143, -0.2143, 0.0, 0.0], + ] # Yaw command: wrench: max: [69.85, 69.85, 49.39, 0, 5, 7] # 5 & 7 are rough estimates diff --git a/auv_setup/config/robots/manta.yaml b/auv_setup/config/robots/manta.yaml index f49b132a6..0836ab26a 100644 --- a/auv_setup/config/robots/manta.yaml +++ b/auv_setup/config/robots/manta.yaml @@ -24,49 +24,108 @@ physical: center_of_buoyancy: [0, 0, 0] drag_coefficients: [80, 80, 204, 1, 1, 1] thruster_layout: - [[-0.70711, 0.70711, 0], - [ 0.00000, 0.00000, 1], - [ 0.00000, 0.00000, 1], - [-0.70711,-0.70711, 0], - [ 0.70711,-0.70711, 0], - [ 0.00000, 0.00000, 1], - [ 0.00000, 0.00000, 1], - [ 0.70711, 0.70711, 0]] + [ + [-0.70711, 0.70711, 0], + [0.00000, 0.00000, 1], + [0.00000, 0.00000, 1], + [-0.70711, -0.70711, 0], + [0.70711, -0.70711, 0], + [0.00000, 0.00000, 1], + [0.00000, 0.00000, 1], + [0.70711, 0.70711, 0], + ] thruster_positions: - [[ 0.20506, 0.20506, 0], - [ 0.12070, 0.12070, 0], - [ 0.12070, -0.12070, 0], - [ 0.20506, -0.20506, 0], - [-0.20506, -0.20506, 0], - [-0.12070, -0.12070, 0], - [-0.12070, 0.12070, 0], - [-0.20506, 0.20506, 0]] + [ + [0.20506, 0.20506, 0], + [0.12070, 0.12070, 0], + [0.12070, -0.12070, 0], + [0.20506, -0.20506, 0], + [-0.20506, -0.20506, 0], + [-0.12070, -0.12070, 0], + [-0.12070, 0.12070, 0], + [-0.20506, 0.20506, 0], + ] propulsion: dofs: num: 6 which: surge: true - sway: true + sway: true heave: true - roll: true + roll: true pitch: true - yaw: true + yaw: true thrusters: num: 8 map: [0, 1, 2, 3, 4, 5, 6, 7] direction: [1, 1, 1, 1, 1, 1, 1, 1] - configuration_matrix: - [[ 0.70711, 0.00000, 0.00000, -0.70711, -0.70711, 0.00000, 0.00000, 0.70711], # Surge - [-0.70711, 0.00000, 0.00000, -0.70711, 0.70711, 0.00000, 0.00000, 0.70711], # Sway - [ 0.00000, 1.00000, 1.00000, 0.00000, 0.00000, 1.00000, 1.00000, 0.00000], # Heave - [ 0.00000, 0.12000, 0.12000, 0.00000, 0.00000, -0.12000, -0.12000, 0.00000], # Roll - [ 0.00000, -0.12000, 0.12000, 0.00000, 0.00000, 0.12000, -0.12000, 0.00000], # Pitch - [-0.29000, -0.00000, -0.00000, 0.29000, -0.29000, 0.00000, 0.00000, 0.29000]] # Yaw + configuration_matrix: [ + [ + 0.70711, + 0.00000, + 0.00000, + -0.70711, + -0.70711, + 0.00000, + 0.00000, + 0.70711, + ], # Surge + [ + -0.70711, + 0.00000, + 0.00000, + -0.70711, + 0.70711, + 0.00000, + 0.00000, + 0.70711, + ], # Sway + [ + 0.00000, + 1.00000, + 1.00000, + 0.00000, + 0.00000, + 1.00000, + 1.00000, + 0.00000, + ], # Heave + [ + 0.00000, + 0.12000, + 0.12000, + 0.00000, + 0.00000, + -0.12000, + -0.12000, + 0.00000, + ], # Roll + [ + 0.00000, + -0.12000, + 0.12000, + 0.00000, + 0.00000, + 0.12000, + -0.12000, + 0.00000, + ], # Pitch + [ + -0.29000, + -0.00000, + -0.00000, + 0.29000, + -0.29000, + 0.00000, + 0.00000, + 0.29000, + ], + ] # Yaw command: wrench: - max: [33.9, 33.9, 48.0, 7.6, 3.2, 9.9] - scaling: [ 0.4, 0.4, 0.6, 0.3, 0.7, 0.4] + max: [33.9, 33.9, 48.0, 7.6, 3.2, 9.9] + scaling: [0.4, 0.4, 0.6, 0.3, 0.7, 0.4] controller: circleOfAcceptance: 0.10 @@ -76,4 +135,4 @@ controller: velocity_gain: 0.7 #0.4 integral_gain: 0.0 #0.3 -computer: 'odroid' +computer: "odroid" diff --git a/auv_setup/config/robots/manta_enu.yaml b/auv_setup/config/robots/manta_enu.yaml index 9d3cce60a..407888c0a 100644 --- a/auv_setup/config/robots/manta_enu.yaml +++ b/auv_setup/config/robots/manta_enu.yaml @@ -18,71 +18,130 @@ # physical: - weight: 18.8 #kg - buoyancy: 18.8 #kg + weight: 18.8 #kg + buoyancy: 18.8 #kg center_of_mass: [0, 0, 0] center_of_buoyancy: [0, 0, 0] drag_coefficients: [80, 80, 204, 1, 1, 1] thruster_layout: - [[-0.70711, 0.70711, 0], - [ 0.00000, 0.00000, 1], - [ 0.00000, 0.00000, 1], - [-0.70711,-0.70711, 0], - [ 0.70711,-0.70711, 0], - [ 0.00000, 0.00000, 1], - [ 0.00000, 0.00000, 1], - [ 0.70711, 0.70711, 0]] + [ + [-0.70711, 0.70711, 0], + [0.00000, 0.00000, 1], + [0.00000, 0.00000, 1], + [-0.70711, -0.70711, 0], + [0.70711, -0.70711, 0], + [0.00000, 0.00000, 1], + [0.00000, 0.00000, 1], + [0.70711, 0.70711, 0], + ] thruster_positions: - [[ 0.20506, 0.20506, 0], - [ 0.12070, 0.12070, 0], - [ 0.12070, -0.12070, 0], - [ 0.20506, -0.20506, 0], - [-0.20506, -0.20506, 0], - [-0.12070, -0.12070, 0], - [-0.12070, 0.12070, 0], - [-0.20506, 0.20506, 0]] + [ + [0.20506, 0.20506, 0], + [0.12070, 0.12070, 0], + [0.12070, -0.12070, 0], + [0.20506, -0.20506, 0], + [-0.20506, -0.20506, 0], + [-0.12070, -0.12070, 0], + [-0.12070, 0.12070, 0], + [-0.20506, 0.20506, 0], + ] propulsion: dofs: num: 6 which: surge: true - sway: true + sway: true heave: true - roll: true + roll: true pitch: true - yaw: true + yaw: true thrusters: num: 8 map: [0, 1, 2, 3, 4, 5, 6, 7] direction: [1, 1, 1, 1, 1, -1, 1, 1] - configuration_matrix: - [[ 0.70711, 0.00000, 0.00000, -0.70711, -0.70711, 0.00000, 0.00000, 0.70711], # Surge - [ 0.70711, 0.00000, 0.00000, 0.70711, -0.70711, 0.00000, 0.00000,-0.70711], # Sway - [ 0.00000, -1.00000, -1.00000, 0.00000, 0.00000, -1.00000, -1.00000, 0.00000], # Heave - [ 0.00000, 0.12000, 0.12000, 0.00000, 0.00000, -0.12000, -0.12000, 0.00000], # Roll - [ 0.00000, 0.12000, -0.12000, 0.00000, 0.00000, -0.12000, 0.12000, 0.00000], # Pitch - [ 0.29000, -0.00000, -0.00000, -0.29000, 0.29000, 0.00000, 0.00000,-0.29000]] # Yaw + configuration_matrix: [ + [ + 0.70711, + 0.00000, + 0.00000, + -0.70711, + -0.70711, + 0.00000, + 0.00000, + 0.70711, + ], # Surge + [ + 0.70711, + 0.00000, + 0.00000, + 0.70711, + -0.70711, + 0.00000, + 0.00000, + -0.70711, + ], # Sway + [ + 0.00000, + -1.00000, + -1.00000, + 0.00000, + 0.00000, + -1.00000, + -1.00000, + 0.00000, + ], # Heave + [ + 0.00000, + 0.12000, + 0.12000, + 0.00000, + 0.00000, + -0.12000, + -0.12000, + 0.00000, + ], # Roll + [ + 0.00000, + 0.12000, + -0.12000, + 0.00000, + 0.00000, + -0.12000, + 0.12000, + 0.00000, + ], # Pitch + [ + 0.29000, + -0.00000, + -0.00000, + -0.29000, + 0.29000, + 0.00000, + 0.00000, + -0.29000, + ], + ] # Yaw rate_of_change: max: 1 # Maximum rate of change in newton per second for a thruster characteristics: # The relationship between thrust in newton and the width in microseconds of the PWM signal to the ESCs thrust: [-12.0, -0.0001, 0.0001, 12.0] - pulse_width: [ 1300, 1475, 1525, 1700] - offset: [-0.20, 1.00, 1.00, -0.35, -0.20, 1.00, 1.00, -0.35] + pulse_width: [1300, 1475, 1525, 1700] + offset: [-0.20, 1.00, 1.00, -0.35, -0.20, 1.00, 1.00, -0.35] command: wrench: - max: [33.9, 33.9, 48.0, 7.6, 3.2, 9.9] - scaling: [ 0.4, 0.4, 0.6, 0.3, 0.7, 0.4] + max: [33.9, 33.9, 48.0, 7.6, 3.2, 9.9] + scaling: [0.4, 0.4, 0.6, 0.3, 0.7, 0.4] controllers: dp: circleOfAcceptance: 0.10 frequency: 40 # THIS SHOULD BE 40Hz for the controller to perform good. This is because the built in reference model will work 2x faster as if the controller ran at 20Hz - velocity_gain: 3.5 #0.4 - position_gain: 30.5 #5.0 - attitude_gain: 1.3 #2.5 + velocity_gain: 3.5 #0.4 + position_gain: 30.5 #5.0 + attitude_gain: 1.3 #2.5 integral_gain: 0.024 #0.3 los_controller: PID: @@ -91,20 +150,20 @@ controllers: d: 3.5 sat: 3.0 backstepping: - c: 3.75 + c: 3.75 k1: 45.0 k2: 28.0 k3: 10.5 - + guidance: LOS: delta: 0.7 - -computer: 'odroid' -torpedoes: # TODO: decide what pins to use for torpedo - left_pin: 23 # both given in BCM scheme - right_pin: 24 +computer: "odroid" + +torpedoes: # TODO: decide what pins to use for torpedo + left_pin: 23 # both given in BCM scheme + right_pin: 24 joystick: scaling: @@ -113,4 +172,4 @@ joystick: heave: 10 roll: 5 pitch: 5 - yaw: 5 \ No newline at end of file + yaw: 5 diff --git a/auv_setup/config/robots/manta_rov.yaml b/auv_setup/config/robots/manta_rov.yaml index abfdf651c..c19f25bca 100644 --- a/auv_setup/config/robots/manta_rov.yaml +++ b/auv_setup/config/robots/manta_rov.yaml @@ -24,49 +24,108 @@ physical: center_of_buoyancy: [0, 0, 0] drag_coefficients: [80, 80, 204, 1, 1, 1] thruster_layout: - [[-0.70711, 0.70711, 0], - [ 0.00000, 0.00000, 1], - [ 0.00000, 0.00000, 1], - [-0.70711,-0.70711, 0], - [ 0.70711,-0.70711, 0], - [ 0.00000, 0.00000, 1], - [ 0.00000, 0.00000, 1], - [ 0.70711, 0.70711, 0]] + [ + [-0.70711, 0.70711, 0], + [0.00000, 0.00000, 1], + [0.00000, 0.00000, 1], + [-0.70711, -0.70711, 0], + [0.70711, -0.70711, 0], + [0.00000, 0.00000, 1], + [0.00000, 0.00000, 1], + [0.70711, 0.70711, 0], + ] thruster_positions: - [[ 0.20506, 0.20506, 0], - [ 0.12070, 0.12070, 0], - [ 0.12070, -0.12070, 0], - [ 0.20506, -0.20506, 0], - [-0.20506, -0.20506, 0], - [-0.12070, -0.12070, 0], - [-0.12070, 0.12070, 0], - [-0.20506, 0.20506, 0]] + [ + [0.20506, 0.20506, 0], + [0.12070, 0.12070, 0], + [0.12070, -0.12070, 0], + [0.20506, -0.20506, 0], + [-0.20506, -0.20506, 0], + [-0.12070, -0.12070, 0], + [-0.12070, 0.12070, 0], + [-0.20506, 0.20506, 0], + ] propulsion: dofs: num: 6 which: surge: true - sway: true + sway: true heave: true - roll: true + roll: true pitch: true - yaw: true + yaw: true thrusters: num: 8 map: [0, 1, 2, 3, 4, 5, 6, 7] direction: [1, 1, 1, 1, 1, -1, 1, 1] - configuration_matrix: - [[ 0.70711, 0.00000, 0.00000, -0.70711, -0.70711, 0.00000, 0.00000, 0.70711], # Surge - [-0.70711, 0.00000, 0.00000, -0.70711, 0.70711, 0.00000, 0.00000, 0.70711], # Sway - [ 0.00000, 1.00000, 1.00000, 0.00000, 0.00000, 1.00000, 1.00000, 0.00000], # Heave - [ 0.00000, 0.12000, 0.12000, 0.00000, 0.00000, -0.12000, -0.12000, 0.00000], # Roll - [ 0.00000, -0.12000, 0.12000, 0.00000, 0.00000, 0.12000, -0.12000, 0.00000], # Pitch - [-0.29000, -0.00000, -0.00000, 0.29000, -0.29000, 0.00000, 0.00000, 0.29000]] # Yaw + configuration_matrix: [ + [ + 0.70711, + 0.00000, + 0.00000, + -0.70711, + -0.70711, + 0.00000, + 0.00000, + 0.70711, + ], # Surge + [ + -0.70711, + 0.00000, + 0.00000, + -0.70711, + 0.70711, + 0.00000, + 0.00000, + 0.70711, + ], # Sway + [ + 0.00000, + 1.00000, + 1.00000, + 0.00000, + 0.00000, + 1.00000, + 1.00000, + 0.00000, + ], # Heave + [ + 0.00000, + 0.12000, + 0.12000, + 0.00000, + 0.00000, + -0.12000, + -0.12000, + 0.00000, + ], # Roll + [ + 0.00000, + -0.12000, + 0.12000, + 0.00000, + 0.00000, + 0.12000, + -0.12000, + 0.00000, + ], # Pitch + [ + -0.29000, + -0.00000, + -0.00000, + 0.29000, + -0.29000, + 0.00000, + 0.00000, + 0.29000, + ], + ] # Yaw command: wrench: - max: [33.9, 33.9, 48.0, 7.6, 3.2, 9.9] - scaling: [ 0.4, 0.4, 0.6, 0.3, 0.7, 0.4] + max: [33.9, 33.9, 48.0, 7.6, 3.2, 9.9] + scaling: [0.4, 0.4, 0.6, 0.3, 0.7, 0.4] controller: frequency: 10 @@ -74,4 +133,4 @@ controller: attitude_gain: 2.5 velocity_gain: 0.4 -computer: 'odroid' +computer: "odroid" diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index e2ba058e5..2f324b54d 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -1,6 +1,6 @@ # This file defines parameters specific to (Insert Name of New AUV). # -# When looking at the AUV from above, the thruster placement is: +# When looking at the AUV from above, the thruster placement is: # # front # |======| @@ -28,56 +28,110 @@ num: 8 min: -100 max: 100 - thruster_force_direction: - [ 0.70711, 0.00000, 0.00000, 0.70711, 0.70711, 0.00000, 0.00000, 0.70711, # Surge - -0.70711, 0.00000, 0.00000, 0.70711, -0.70711, 0.00000, 0.00000, 0.70711, # Sway - 0.00000, 1.00000, 1.00000, 0.00000, 0.00000, 1.00000, 1.00000, 0.00000 ] # Heave - thruster_position: - [ 0.41500, 0.28400, -0.31800, -0.44900, -0.44900, -0.31800, 0.28400, 0.41500, # x-position of thrusters - 0.16900, 0.16300, 0.16300, 0.16900, -0.16900, -0.16300, -0.16300, -0.16900, # y-position of thrusters - 0.07600, 0.08200, 0.08200, 0.07600, 0.07600, 0.08200, 0.08200, 0.07600 ] # z-position of thrusters + thruster_force_direction: [ + 0.70711, + 0.00000, + 0.00000, + 0.70711, + 0.70711, + 0.00000, + 0.00000, + 0.70711, # Surge + -0.70711, + 0.00000, + 0.00000, + 0.70711, + -0.70711, + 0.00000, + 0.00000, + 0.70711, # Sway + 0.00000, + 1.00000, + 1.00000, + 0.00000, + 0.00000, + 1.00000, + 1.00000, + 0.00000, + ] # Heave + thruster_position: [ + 0.41500, + 0.28400, + -0.31800, + -0.44900, + -0.44900, + -0.31800, + 0.28400, + 0.41500, # x-position of thrusters + 0.16900, + 0.16300, + 0.16300, + 0.16900, + -0.16900, + -0.16300, + -0.16300, + -0.16900, # y-position of thrusters + 0.07600, + 0.08200, + 0.08200, + 0.07600, + 0.07600, + 0.08200, + 0.08200, + 0.07600, + ] # z-position of thrusters rate_of_change: max: 1 # Maximum rate of change in newton per second for a thruster - + thrust_update_rate: 10.0 # [Hz] - + thruster_to_pin_mapping: [0, 1, 2, 3, 4, 6, 5, 7] # I.e. if thruster_to_pin = [ 7, 6, 5, 4, 3, 2, 1, 0 ] then thruster 0 is pin 1 etc.. - thruster_direction: [1, -1, 1, -1, 1, 1, 1, -1] # Disclose during thruster mapping (+/- 1) + thruster_direction: [1, -1, 1, -1, 1, 1, 1, -1] # Disclose during thruster mapping (+/- 1) thruster_PWM_offset: [-28, -28, -28, -28, -28, -28, -28, -28] # Offset IN PWM -400 to 400 # Offset moves where the thrust is at rest thruster_PWM_min: [1200, 1200, 1200, 1200, 1200, 1200, 1200, 1200] # Minimum PWM value, Recommended: [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100] thruster_PWM_max: [1800, 1800, 1800, 1800, 1800, 1800, 1800, 1800] # Maximum PWM value, Recommended: [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900] - + internal_status: power_sense_module_read_rate: 5.0 # Readings/second, Recommended: 5.0 voltage_min: 14.5 # [V], Recommended: 14.5 voltage_max: 16.8 # [V], Recommended: 16.8 power_sense_module_warning_rate: 0.1 # Warnings/second in case something goes bad you have a warning, Recommended: 0.1 - + pressure_read_rate: 1.0 # Readings/second, Recommended: 1.0 pressure_critical_level: 900.0 # [hPa] If the pressure is over this amount there might be a leakage, Recommended: <1013.25 pressure_warning_rate: 0.1 # Warnings/second in case something goes bad you have a warning, Recommended: 0.1 - + temperature_read_rate: 0.1 # Readings/second, Recommended: 0.1 temperature_critical_level: 90.0 # [*C] If temperature is over this amount the electrical housing might be overheating, Recommended: 90.0 temperature_warning_rate: 0.1 # Warnings/second in case something goes bad you have a warning, Recommended: 0.1 - + blackbox: data_logging_rate: 5.0 # [loggings/second], Recommended: 5.0 loggings per second - + acoustics: frequencies_of_interest: # MUST be 20 elements [FREQUENCY [Hz], FREQUENCY_VARIANCE [Hz]...] Recommended: Frequency 1 000 - 50 000 Hz, Variance 1 000 - 10 000 Hz - [10000, 1000, - 50000, 3000, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0] + [ + 10000, + 1000, + 50000, + 3000, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + ] data_logging_rate: 1.0 # [loggings/second], Recommended: 1.0 loggings per second - - diff --git a/auv_setup/config/robots/terrapin.yaml b/auv_setup/config/robots/terrapin.yaml index e2f0f6e6e..6ae81ecdb 100644 --- a/auv_setup/config/robots/terrapin.yaml +++ b/auv_setup/config/robots/terrapin.yaml @@ -18,24 +18,79 @@ propulsion: num: 6 which: surge: true - sway: true + sway: true heave: true - roll: true + roll: true pitch: true - yaw: true + yaw: true thrusters: num: 8 - configuration_matrix: - [[ 0.70711, 0.00000, 0.00000, -0.70711, -0.70711, 0.00000, 0.00000, 0.70711], # Surge - [-0.70711, 0.00000, 0.00000, -0.70711, 0.70711, 0.00000, 0.00000, 0.70711], # Sway - [ 0.00000, 1.00000, 1.00000, 0.00000, 0.00000, 1.00000, 1.00000, 0.00000], # Heave - [ 0.00000, 0.15800, 0.15800, 0.00000, 0.00000, -0.15800, -0.15800, 0.00000], # Roll - [ 0.00000, -0.06600, 0.06600, 0.00000, 0.00000, 0.06600, -0.06600, 0.00000], # Pitch - [-0.20577, -0.00000, -0.00000, 0.20577, -0.20577, 0.00000, 0.00000, 0.20577]] # Yaw + configuration_matrix: [ + [ + 0.70711, + 0.00000, + 0.00000, + -0.70711, + -0.70711, + 0.00000, + 0.00000, + 0.70711, + ], # Surge + [ + -0.70711, + 0.00000, + 0.00000, + -0.70711, + 0.70711, + 0.00000, + 0.00000, + 0.70711, + ], # Sway + [ + 0.00000, + 1.00000, + 1.00000, + 0.00000, + 0.00000, + 1.00000, + 1.00000, + 0.00000, + ], # Heave + [ + 0.00000, + 0.15800, + 0.15800, + 0.00000, + 0.00000, + -0.15800, + -0.15800, + 0.00000, + ], # Roll + [ + 0.00000, + -0.06600, + 0.06600, + 0.00000, + 0.00000, + 0.06600, + -0.06600, + 0.00000, + ], # Pitch + [ + -0.20577, + -0.00000, + -0.00000, + 0.20577, + -0.20577, + 0.00000, + 0.00000, + 0.20577, + ], + ] # Yaw command: wrench: - max: [33.9, 33.9, 48.0, 7.6, 3.2, 9.9] - scaling: [ 0.4, 0.4, 0.6, 0.3, 0.7, 0.2] + max: [33.9, 33.9, 48.0, 7.6, 3.2, 9.9] + scaling: [0.4, 0.4, 0.6, 0.3, 0.7, 0.2] pose: rate: [0.20, 0.20, 0.20, 0.35, 0.35, 0.35] # Euler angle pose! @@ -55,12 +110,12 @@ stepper: default_speed_rpm: 30 steps_per_rev: 200 pins: - valve: ['P8_28', 'P8_30', 'P8_27', 'P8_29'] - agar: ['P8_32', 'P8_34', 'P8_31', 'P8_33'] - valve_enable: 'P8_46' - agar_enable: 'P8_44' + valve: ["P8_28", "P8_30", "P8_27", "P8_29"] + agar: ["P8_32", "P8_34", "P8_31", "P8_33"] + valve_enable: "P8_46" + agar_enable: "P8_44" -computer: 'beaglebone' +computer: "beaglebone" pwm: pins: @@ -80,11 +135,11 @@ camera: pin_map_feed2: ["P8_15", "P8_14", "P8_13"] light: - gpio_pins: {"bluetooth":"P8_16", "raman":"P8_17"} - pwm_pins: {"front":8} + gpio_pins: { "bluetooth": "P8_16", "raman": "P8_17" } + pwm_pins: { "front": 8 } sensors: bno055: # https://github.com/adafruit/Adafruit_Python_BNO055 - mode: 'IMU' + mode: "IMU" # mode: 'NDOF' From aad0bfc108841bfe88e35eefdaa898c00506cf58 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Wed, 25 Sep 2024 17:56:51 +0200 Subject: [PATCH 48/51] ci: add pipeline for yaml formatting using prettier --- .github/workflows/yaml-format.yaml | 31 ++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 .github/workflows/yaml-format.yaml diff --git a/.github/workflows/yaml-format.yaml b/.github/workflows/yaml-format.yaml new file mode 100644 index 000000000..3745f7eb6 --- /dev/null +++ b/.github/workflows/yaml-format.yaml @@ -0,0 +1,31 @@ +name: Yaml Format (prettier) + +on: [push] + +jobs: + prettier: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v4 + with: + ref: ${{ github.head_ref }} + + - name: Install Prettier + run: | + sudo npm install -g prettier + + # Run Prettier to format all YAML files + - name: Run Prettier to format YAML files + run: | + prettier --write "**/*.yaml" "**/*.yml" + + # Commit Prettier changes + - name: Commit Prettier changes + uses: EndBug/add-and-commit@v9 + with: + author_name: Prettier Robot + author_email: prettier-robot@example.com + message: "Committing Prettier formatting fixes" + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} From fd63bea5f2e612e65072c62035f7c114791762e0 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Wed, 25 Sep 2024 17:58:26 +0200 Subject: [PATCH 49/51] ci: update yaml formatting pipeline to run on pull request --- .github/workflows/yaml-format.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/yaml-format.yaml b/.github/workflows/yaml-format.yaml index 3745f7eb6..a0a5745f0 100644 --- a/.github/workflows/yaml-format.yaml +++ b/.github/workflows/yaml-format.yaml @@ -1,6 +1,6 @@ name: Yaml Format (prettier) -on: [push] +on: [pull_request] jobs: prettier: From 99c56460bb379e639656fa791635c94926a5f27f Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Fri, 27 Sep 2024 14:04:47 +0200 Subject: [PATCH 50/51] refactor: remove deprecated typing --- .../acoustics_data_record_lib.py | 22 +++++++++---------- .../thruster_interface_auv_driver_lib.py | 12 +++++----- 2 files changed, 15 insertions(+), 19 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 0fdb53b07..822c0edd9 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 @@ -2,8 +2,6 @@ import csv import time from datetime import datetime -from typing import List - class AcousticsDataRecordLib: def __init__(self, ros2_package_directory: str = "") -> None: @@ -37,16 +35,16 @@ def __init__(self, ros2_package_directory: str = "") -> None: # Methods for external uses ---------- def log_data_to_csv_file( self, - hydrophone1: List[int] = [0], - hydrophone2: List[int] = [0], - hydrophone3: List[int] = [0], - hydrophone4: List[int] = [0], - hydrophone5: List[int] = [0], - filter_response: List[int] = [0], - fft: List[int] = [0], - peaks: List[int] = [0], - tdoa: List[float] = [0.0], - position: List[float] = [0.0], + hydrophone1: list[int] = [0], + hydrophone2: list[int] = [0], + hydrophone3: list[int] = [0], + hydrophone4: list[int] = [0], + hydrophone5: list[int] = [0], + filter_response: list[int] = [0], + fft: list[int] = [0], + peaks: list[int] = [0], + tdoa: list[float] = [0.0], + position: list[float] = [0.0], ) -> None: """ Logs the provided data to a CSV 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 6511fbe5f..d74713dd5 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,7 +1,5 @@ #!/usr/bin/env python3 # Import libraries -from typing import List - import numpy import pandas import smbus2 @@ -14,11 +12,11 @@ def __init__( 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], + 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 From bcd5cc41fd8131cc7ffd50cda0724f5345701e83 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Fri, 27 Sep 2024 14:07:18 +0200 Subject: [PATCH 51/51] refactor: fix import sorting --- .../acoustics_data_record/acoustics_data_record_lib.py | 1 + 1 file changed, 1 insertion(+) 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 822c0edd9..7d921d65e 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 @@ -3,6 +3,7 @@ import time from datetime import datetime + class AcousticsDataRecordLib: def __init__(self, ros2_package_directory: str = "") -> None: # Global variables for .csv file manipulation ----------