From aa75f5341299f2126f5fe86eb4511b23b9c9f682 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Fri, 8 Nov 2024 15:52:51 +0100 Subject: [PATCH] refactor: comprehensive linting, formatting, and ci/cd pipeline improvements * Added and configured linting tools (.pylintrc, codespell, black, isort) for consistent code quality * Fixed multiple pylint warnings and applied standard formatting across files * Updated CI/CD pipelines to incorporate linting, formatting, and type-checking (pylint, black, isort, mypy) * Added pre-commit hooks to automate linting and formatting on commit * Introduced a simple GUI and 3D plotting capabilities for drone position, pose, and internal status display * Configured and enhanced build/test pipelines, including security improvements (safer subprocess handling) * Applied multiple refactors to improve code readability, variable naming, and import sorting * Standardized YAML and other configuration files using prettier formatting --- .clang-format | 318 ++++++++++---- .github/workflows/black-formatter.yml | 28 -- .github/workflows/ci-pre-commit.yaml | 11 + .github/workflows/clang-formatter.yml | 24 -- .github/workflows/docker-publish.yml | 60 --- .github/workflows/semver.yaml | 20 + .github/workflows/source-build.yaml | 60 +-- .github/workflows/update-pre-commit.yaml | 13 + .gitignore | 2 +- .gitmodules | 3 - .pre-commit-config.yaml | 83 ++++ .releaserc | 8 + README.md | 1 - .../acoustics_data_record/CMakeLists.txt | 2 +- acoustics/acoustics_data_record/README | 2 +- .../acoustics_data_record_lib.py | 57 ++- .../acoustics_data_record_node.py | 216 ++++++---- ...nch.py => acoustics_data_record_launch.py} | 20 +- .../utilities/display_acoustics_data_live.py | 347 +++++++++------ acoustics/acoustics_interface/CMakeLists.txt | 2 +- acoustics/acoustics_interface/README | 4 +- .../acoustics_interface_lib.py | 115 +++-- .../acoustics_interface_node.py | 57 ++- .../launch/acoustics_interface_launch.py | 20 +- auv_setup/CMakeLists.txt | 7 - auv_setup/config/robots/beluga.yaml | 187 +++++--- 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 | 143 +++++-- auv_setup/config/robots/terrapin.yaml | 95 +++- auv_setup/launch/orca.launch.py | 19 +- auv_setup/launch/topside.launch.py | 17 +- mission/LCD/README | 8 +- mission/LCD/sources/IP_lib.py | 14 - mission/LCD/sources/LCD.py | 69 --- mission/LCD/sources/ip_lib.py | 28 ++ mission/LCD/sources/lcd.py | 79 ++++ .../LCD/sources/{LCD_lib.py => lcd_lib.py} | 86 ++-- mission/LCD/sources/power_sense_module_lib.py | 31 +- mission/LCD/sources/pressure_sensor_lib.py | 20 +- mission/LCD/sources/temperature_sensor_lib.py | 23 +- mission/LCD/startup_scripts/LCD.service | 6 +- mission/blackbox/README | 6 +- .../blackbox/blackbox/blackbox_log_data.py | 91 ++-- mission/blackbox/blackbox/blackbox_node.py | 114 ++++- mission/blackbox/blackbox_data/.gitignore | 2 +- mission/blackbox/launch/blackbox.launch.py | 16 +- mission/blackbox/package.xml | 2 +- .../blackbox/startup_scripts/blackbox.service | 4 +- mission/blackbox/startup_scripts/blackbox.sh | 2 +- mission/gui_auv/auv_gui/__init__.py | 0 mission/gui_auv/auv_gui/auv_gui.py | 317 ++++++++++++++ mission/gui_auv/config/gui_params.yaml | 9 + mission/gui_auv/package.xml | 19 + mission/gui_auv/setup.py | 21 + mission/internal_status_auv/README.md | 8 +- .../power_sense_module_lib.py | 35 +- .../power_sense_module_node.py | 50 ++- .../pressure_sensor_lib.py | 26 +- .../pressure_sensor_node.py | 44 +- .../temperature_sensor_lib.py | 25 +- .../temperature_sensor_node.py | 44 +- .../launch/internal_status_auv.launch.py | 22 +- .../internal_status_auv.service | 6 +- .../startup_scripts/internal_status_auv.sh | 2 +- mission/joystick_interface_auv/README.md | 1 - .../joystick_interface_auv.py | 58 ++- .../launch/joystick_interface_auv.launch.py | 15 +- mission/joystick_interface_auv/package.xml | 8 +- .../tests/test_joystick_interface_auv.py | 83 +++- motion/thrust_allocator_auv/CMakeLists.txt | 12 +- .../eigen_vector6d_typedef.hpp | 2 +- .../pseudoinverse_allocator.hpp | 36 +- .../thrust_allocator_ros.hpp | 88 ++-- .../thrust_allocator_utils.hpp | 128 +++--- .../launch/thrust_allocator_auv.launch.py | 15 +- .../src/pseudoinverse_allocator.cpp | 12 +- .../src/thrust_allocator_auv_node.cpp | 16 +- .../src/thrust_allocator_ros.cpp | 176 ++++---- .../launch/thruster_interface_auv.launch.py | 18 +- motion/thruster_interface_auv/package.xml | 4 +- .../thruster_interface_auv/resources/README | 2 +- .../resources/T200-Thrusters-10V.csv | 404 +++++++++--------- .../resources/T200-Thrusters-12V.csv | 404 +++++++++--------- .../resources/T200-Thrusters-14V.csv | 404 +++++++++--------- .../resources/T200-Thrusters-16V.csv | 404 +++++++++--------- .../resources/T200-Thrusters-18V.csv | 404 +++++++++--------- .../resources/T200-Thrusters-20V.csv | 404 +++++++++--------- .../thruster_interface_auv_driver_lib.py | 139 +++--- .../thruster_interface_auv_node.py | 41 +- perception-auv | 1 - ruff.toml | 6 + scripts/activate_thrusters.sh | 13 +- .../add_service_files_to_bootup_sequence.sh | 4 +- scripts/fsm.sh | 4 +- scripts/perception_sim.sh | 12 +- 98 files changed, 4190 insertions(+), 2698 deletions(-) delete mode 100644 .github/workflows/black-formatter.yml create mode 100644 .github/workflows/ci-pre-commit.yaml delete mode 100644 .github/workflows/clang-formatter.yml delete mode 100644 .github/workflows/docker-publish.yml create mode 100644 .github/workflows/semver.yaml create mode 100644 .github/workflows/update-pre-commit.yaml delete mode 100644 .gitmodules create mode 100644 .pre-commit-config.yaml create mode 100644 .releaserc rename acoustics/acoustics_data_record/launch/{acoustics_data_record.launch.py => acoustics_data_record_launch.py} (63%) delete mode 100644 mission/LCD/sources/IP_lib.py delete mode 100644 mission/LCD/sources/LCD.py create mode 100644 mission/LCD/sources/ip_lib.py create mode 100644 mission/LCD/sources/lcd.py rename mission/LCD/sources/{LCD_lib.py => lcd_lib.py} (53%) create mode 100644 mission/gui_auv/auv_gui/__init__.py create mode 100644 mission/gui_auv/auv_gui/auv_gui.py create mode 100644 mission/gui_auv/config/gui_params.yaml create mode 100644 mission/gui_auv/package.xml create mode 100644 mission/gui_auv/setup.py delete mode 160000 perception-auv create mode 100644 ruff.toml diff --git a/.clang-format b/.clang-format index 33bf2a3b9..8340806b9 100644 --- a/.clang-format +++ b/.clang-format @@ -1,137 +1,307 @@ --- -Language: Cpp -# BasedOnStyle: LLVM -AccessModifierOffset: -2 +BasedOnStyle: Chromium +AccessModifierOffset: -1 AlignAfterOpenBracket: Align -AlignConsecutiveMacros: false -AlignConsecutiveAssignments: false -AlignConsecutiveDeclarations: false -AlignEscapedNewlines: Right -AlignOperands: true -AlignTrailingComments: true +AlignArrayOfStructures: None +AlignConsecutiveAssignments: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: true +AlignConsecutiveBitFields: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveDeclarations: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveMacros: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveShortCaseStatements: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCaseArrows: false + AlignCaseColons: false +AlignConsecutiveTableGenBreakingDAGArgColons: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveTableGenCondOperatorColons: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveTableGenDefinitionColons: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignEscapedNewlines: Left +AlignOperands: Align +AlignTrailingComments: + Kind: Always + OverEmptyLines: 0 AllowAllArgumentsOnNextLine: true -AllowAllConstructorInitializersOnNextLine: true -AllowAllParametersOfDeclarationOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowBreakBeforeNoexceptSpecifier: Never AllowShortBlocksOnASingleLine: Never +AllowShortCaseExpressionOnASingleLine: true AllowShortCaseLabelsOnASingleLine: false -AllowShortFunctionsOnASingleLine: All -AllowShortLambdasOnASingleLine: All +AllowShortCompoundRequirementOnASingleLine: true +AllowShortEnumsOnASingleLine: true +AllowShortFunctionsOnASingleLine: Inline AllowShortIfStatementsOnASingleLine: Never +AllowShortLambdasOnASingleLine: All AllowShortLoopsOnASingleLine: false AlwaysBreakAfterDefinitionReturnType: None -AlwaysBreakAfterReturnType: None -AlwaysBreakBeforeMultilineStrings: false -AlwaysBreakTemplateDeclarations: MultiLine +AlwaysBreakBeforeMultilineStrings: true +AttributeMacros: + - __capability BinPackArguments: true -BinPackParameters: true +BinPackParameters: false +BitFieldColonSpacing: Both BraceWrapping: - AfterCaseLabel: false - AfterClass: false - AfterControlStatement: false - AfterEnum: false - AfterFunction: false - AfterNamespace: false + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: Never + AfterEnum: false + AfterFunction: false + AfterNamespace: false AfterObjCDeclaration: false - AfterStruct: false - AfterUnion: false + AfterStruct: false + AfterUnion: false AfterExternBlock: false - BeforeCatch: false - BeforeElse: false - IndentBraces: false + BeforeCatch: false + BeforeElse: false + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: false SplitEmptyFunction: true SplitEmptyRecord: true SplitEmptyNamespace: true +BreakAdjacentStringLiterals: true +BreakAfterAttributes: Leave +BreakAfterJavaFieldAnnotations: false +BreakAfterReturnType: None +BreakArrays: true BreakBeforeBinaryOperators: None BreakBeforeBraces: Attach -BreakBeforeInheritanceComma: false -BreakInheritanceList: BeforeColon +BreakBeforeConceptDeclarations: Always +BreakBeforeInlineASMColon: OnlyMultiline BreakBeforeTernaryOperators: true -BreakConstructorInitializersBeforeComma: false BreakConstructorInitializers: BeforeColon -BreakAfterJavaFieldAnnotations: false +BreakFunctionDefinitionParameters: false +BreakInheritanceList: BeforeColon BreakStringLiterals: true -ColumnLimit: 80 -CommentPragmas: '^ IWYU pragma:' +BreakTemplateDeclarations: Yes +ColumnLimit: 80 +CommentPragmas: "^ IWYU pragma:" CompactNamespaces: false -ConstructorInitializerAllOnOneLineOrOnePerLine: false ConstructorInitializerIndentWidth: 4 ContinuationIndentWidth: 4 Cpp11BracedListStyle: true -DeriveLineEnding: true DerivePointerAlignment: false -DisableFormat: false +DisableFormat: false +EmptyLineAfterAccessModifier: Never +EmptyLineBeforeAccessModifier: LogicalBlock ExperimentalAutoDetectBinPacking: false FixNamespaceComments: true ForEachMacros: - foreach - Q_FOREACH - BOOST_FOREACH -IncludeBlocks: Preserve +IfMacros: + - KJ_IF_MAYBE +IncludeBlocks: Preserve IncludeCategories: - - Regex: '^"(llvm|llvm-c|clang|clang-c)/' - Priority: 2 - SortPriority: 0 - - Regex: '^(<|"(gtest|gmock|isl|json)/)' - Priority: 3 - SortPriority: 0 - - Regex: '.*' - Priority: 1 - SortPriority: 0 -IncludeIsMainRegex: '(Test)?$' -IncludeIsMainSourceRegex: '' -IndentCaseLabels: false + - Regex: ^ + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: ^<.*\.h> + Priority: 1 + SortPriority: 0 + CaseSensitive: false + - Regex: ^<.* + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: .* + Priority: 3 + SortPriority: 0 + CaseSensitive: false +IncludeIsMainRegex: ([-_](test|unittest))?$ +IncludeIsMainSourceRegex: "" +IndentAccessModifiers: false +IndentCaseBlocks: false +IndentCaseLabels: true +IndentExternBlock: AfterExternBlock IndentGotoLabels: true IndentPPDirectives: None -IndentWidth: 2 +IndentRequiresClause: true +IndentWidth: 4 IndentWrappedFunctionNames: false +InsertBraces: false +InsertNewlineAtEOF: false +InsertTrailingCommas: None +IntegerLiteralSeparator: + Binary: 0 + BinaryMinDigits: 0 + Decimal: 0 + DecimalMinDigits: 0 + Hex: 0 + HexMinDigits: 0 JavaScriptQuotes: Leave JavaScriptWrapImports: true -KeepEmptyLinesAtTheStartOfBlocks: true -MacroBlockBegin: '' -MacroBlockEnd: '' +KeepEmptyLines: + AtEndOfFile: false + AtStartOfBlock: false + AtStartOfFile: true +LambdaBodyIndentation: Signature +Language: Cpp +LineEnding: DeriveLF +MacroBlockBegin: "" +MacroBlockEnd: "" +MainIncludeChar: Quote MaxEmptyLinesToKeep: 1 NamespaceIndentation: None -ObjCBinPackProtocolList: Auto +ObjCBinPackProtocolList: Never ObjCBlockIndentWidth: 2 +ObjCBreakBeforeNestedBlockParam: true ObjCSpaceAfterProperty: false ObjCSpaceBeforeProtocolList: true +PPIndentWidth: -1 +PackConstructorInitializers: NextLine PenaltyBreakAssignment: 2 -PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakBeforeFirstCallParameter: 1 PenaltyBreakComment: 300 PenaltyBreakFirstLessLess: 120 +PenaltyBreakOpenParenthesis: 0 +PenaltyBreakScopeResolution: 500 PenaltyBreakString: 1000 PenaltyBreakTemplateDeclaration: 10 PenaltyExcessCharacter: 1000000 -PenaltyReturnTypeOnItsOwnLine: 60 -PointerAlignment: Right -ReflowComments: true -SortIncludes: true -SortUsingDeclarations: true +PenaltyIndentedWhitespace: 0 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +QualifierAlignment: Leave +RawStringFormats: + - Language: Cpp + Delimiters: + - cc + - CC + - cpp + - Cpp + - CPP + - c++ + - C++ + CanonicalDelimiter: "" + BasedOnStyle: google + - Language: TextProto + Delimiters: + - pb + - PB + - proto + - PROTO + EnclosingFunctions: + - EqualsProto + - EquivToProto + - PARSE_PARTIAL_TEXT_PROTO + - PARSE_TEST_PROTO + - PARSE_TEXT_PROTO + - ParseTextOrDie + - ParseTextProtoOrDie + - ParseTestProto + - ParsePartialTestProto + CanonicalDelimiter: pb + BasedOnStyle: google +ReferenceAlignment: Pointer +ReflowComments: true +RemoveBracesLLVM: false +RemoveParentheses: Leave +RemoveSemicolon: false +RequiresClausePosition: OwnLine +RequiresExpressionIndentation: OuterScope +SeparateDefinitionBlocks: Leave +ShortNamespaceLines: 1 +SkipMacroDefinitionBody: false +SortIncludes: CaseSensitive +SortJavaStaticImport: Before +SortUsingDeclarations: LexicographicNumeric SpaceAfterCStyleCast: false SpaceAfterLogicalNot: false SpaceAfterTemplateKeyword: true +SpaceAroundPointerQualifiers: Default SpaceBeforeAssignmentOperators: true +SpaceBeforeCaseColon: false SpaceBeforeCpp11BracedList: false SpaceBeforeCtorInitializerColon: true SpaceBeforeInheritanceColon: true +SpaceBeforeJsonColon: false SpaceBeforeParens: ControlStatements +SpaceBeforeParensOptions: + AfterControlStatements: true + AfterForeachMacros: true + AfterFunctionDeclarationName: false + AfterFunctionDefinitionName: false + AfterIfMacros: true + AfterOverloadedOperator: false + AfterPlacementOperator: true + AfterRequiresInClause: false + AfterRequiresInExpression: false + BeforeNonEmptyParentheses: false SpaceBeforeRangeBasedForLoopColon: true +SpaceBeforeSquareBrackets: false SpaceInEmptyBlock: false -SpaceInEmptyParentheses: false -SpacesBeforeTrailingComments: 1 -SpacesInAngles: false -SpacesInConditionalStatement: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: Never SpacesInContainerLiterals: true -SpacesInCStyleCastParentheses: false -SpacesInParentheses: false +SpacesInLineCommentPrefix: + Minimum: 1 + Maximum: -1 +SpacesInParens: Never +SpacesInParensOptions: + ExceptDoubleParentheses: false + InConditionalStatements: false + InCStyleCasts: false + InEmptyParentheses: false + Other: false SpacesInSquareBrackets: false -SpaceBeforeSquareBrackets: false -Standard: Latest +Standard: Auto +StatementAttributeLikeMacros: + - Q_EMIT StatementMacros: - Q_UNUSED - QT_REQUIRE_VERSION -TabWidth: 8 -UseCRLF: false -UseTab: Never -... - +TabWidth: 8 +TableGenBreakInsideDAGArg: DontBreak +UseTab: Never +VerilogBreakBetweenInstancePorts: true +WhitespaceSensitiveMacros: + - BOOST_PP_STRINGIZE + - CF_SWIFT_NAME + - NS_SWIFT_NAME + - PP_STRINGIZE + - STRINGIZE 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/ci-pre-commit.yaml b/.github/workflows/ci-pre-commit.yaml new file mode 100644 index 000000000..642bb7fb4 --- /dev/null +++ b/.github/workflows/ci-pre-commit.yaml @@ -0,0 +1,11 @@ +name: Run Pre-Commit Workflow +# Run pre-commit hooks on a repository +on: + workflow_dispatch: + pull_request: +jobs: + call_reusable_workflow: + uses: vortexntnu/vortex-ci/.github/workflows/reusable-pre-commit.yaml@main + with: + ref: ${{ github.ref }} + python_version: '3.11' diff --git a/.github/workflows/clang-formatter.yml b/.github/workflows/clang-formatter.yml deleted file mode 100644 index 535afdac3..000000000 --- a/.github/workflows/clang-formatter.yml +++ /dev/null @@ -1,24 +0,0 @@ -name: Run clang-format Linter - -on: [pull_request] - -jobs: - build: - 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 diff --git a/.github/workflows/docker-publish.yml b/.github/workflows/docker-publish.yml deleted file mode 100644 index 87d9085ca..000000000 --- a/.github/workflows/docker-publish.yml +++ /dev/null @@ -1,60 +0,0 @@ -name: Publish Docker image to ghcr - -on: - push: - branches: [ development ] - -env: - REGISTRY: ghcr.io - IMAGE_NAME: ${{ github.repository }} - -jobs: - build-and-push-image: - runs-on: ubuntu-latest - permissions: - contents: read - packages: write - - steps: - - name: Checkout own repository - uses: actions/checkout@v3 - - # vortex_msgs needs to be in the same location as the rest of the vortex-auv packages - - name: Checkout vortex_msgs - uses: actions/checkout@v3 - with: - repository: 'vortexntnu/vortex-msgs' - path: './vortex-msgs' - - name: Move vortex_msgs - run: mv ./vortex-msgs ../vortex-msgs - - # robot_localization needs to be in the same location as the rest of the vortex-auv packages - - name: Checkout robot_localization - uses: actions/checkout@v3 - with: - repository: 'vortexntnu/robot_localization' - path: './robot_localization' - - name: Move robot_localization - run: mv ./robot_localization ../robot_localization - - - name: Log in to the Container registry - uses: docker/login-action@f054a8b539a109f9f41c372932f1ae047eff08c9 - with: - registry: ${{ env.REGISTRY }} - username: ${{ github.actor }} - password: ${{ secrets.GITHUB_TOKEN }} - - - name: Extract metadata (tags, labels) for Docker - id: meta - uses: docker/metadata-action@98669ae865ea3cffbcbaa878cf57c20bbf1c6c38 - with: - images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} - - - name: Build and push Docker image - uses: docker/build-push-action@ad44023a93711e3deb337508980b4b5e9bcdc5dc - with: - context: .. - file: ./docker/Dockerfile - push: true - tags: ${{ steps.meta.outputs.tags }} - labels: ${{ steps.meta.outputs.labels }} diff --git a/.github/workflows/semver.yaml b/.github/workflows/semver.yaml new file mode 100644 index 000000000..4210500ac --- /dev/null +++ b/.github/workflows/semver.yaml @@ -0,0 +1,20 @@ +name: Semantic Versioning +on: + push: + branches: + - main + +jobs: + publish: + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Semantic Release + uses: cycjimmy/semantic-release-action@v4.1.1 + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + with: + extra_plugins: | + @semantic-release/github@v11.0.0 diff --git a/.github/workflows/source-build.yaml b/.github/workflows/source-build.yaml index 6166fbe6f..802f53637 100644 --- a/.github/workflows/source-build.yaml +++ b/.github/workflows/source-build.yaml @@ -1,54 +1,16 @@ name: Source Build - +# Build the ROS 2 workspace from source code and run tests on: + workflow_dispatch: pull_request: - - # runs daily to check for dependency issues or flaking tests + # Runs daily to check for dependency issues or flaking tests schedule: - - cron: "0 3 * * *" - + - cron: "0 1 * * *" jobs: - build: - runs-on: ubuntu-22.04 - - steps: - - name: Setup ROS 2 Environment - uses: ros-tooling/setup-ros@v0.7 - with: - required-ros-distributions: humble - - - name: Checkout Repository - uses: actions/checkout@v4 - with: - path: ros2_ws/src/${{ github.event.repository.name }} - - - name: Import Dependencies with vcs - run: | - cd ${{ github.workspace }}/ros2_ws/src - vcs import . < ${{ github.workspace }}/ros2_ws/src/${{ github.event.repository.name }}/ros2.repos - - - name: Install Package Dependencies with rosdep - run: | - cd ${{ github.workspace }}/ros2_ws - source /opt/ros/humble/setup.bash - - rosdep update - rosdep install --from-paths src --ignore-src -r -y - - - name: Build ROS 2 Workspace - run: | - cd ${{ github.workspace }}/ros2_ws - source /opt/ros/humble/setup.bash - colcon build --event-handlers console_cohesion+ - - - name: Run Tests - run: | - cd ${{ github.workspace }}/ros2_ws - source /opt/ros/humble/setup.bash - colcon test --event-handlers console_direct+ - - - name: Test Results Summary - run: | - cd ${{ github.workspace }}/ros2_ws - source /opt/ros/humble/setup.bash - colcon test-result --verbose + source-build: + uses: vortexntnu/vortex-ci/.github/workflows/reusable-source-build.yaml@main + with: + ros_distro: 'humble' + os_name: 'ubuntu-22.04' + ref: ${{ github.ref_name }} + vcs_repo_file_url: 'https://raw.githubusercontent.com/vortexntnu/vortex-auv/main/ros2.repos' diff --git a/.github/workflows/update-pre-commit.yaml b/.github/workflows/update-pre-commit.yaml new file mode 100644 index 000000000..1246b33f2 --- /dev/null +++ b/.github/workflows/update-pre-commit.yaml @@ -0,0 +1,13 @@ +name: Run Pre-Commit Update Workflow +# Update pre-commit config and create a pull request if changes are detected +on: + workflow_dispatch: + schedule: + - cron: "0 0 1 * *" # Run every month at midnight on the first day of the month +jobs: + call_reusable_workflow: + uses: vortexntnu/vortex-ci/.github/workflows/reusable-update-pre-commit.yaml@main + with: + ref: ${{ github.ref }} + python_version: '3.10' + responsible: 'kluge7' diff --git a/.gitignore b/.gitignore index cf771bc2b..f86dbd841 100644 --- a/.gitignore +++ b/.gitignore @@ -6,7 +6,7 @@ cmake-build-debug/ ### ROS ### install/ -logs/ +log/ build/ bin/ lib/ diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index 8d1d96bd4..000000000 --- a/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "perception-auv"] - path = perception-auv - url = git@github.com:vortexntnu/perception-auv.git diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 000000000..c867df9f1 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,83 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v5.0.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-docstring-first + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + args: ["--allow-multiple-documents"] + - id: debug-statements + - id: end-of-file-fixer + - id: mixed-line-ending + - id: trailing-whitespace + exclude_types: [rst] + - id: fix-byte-order-marker + - id: requirements-txt-fixer + # Python hooks + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.7.0 + hooks: + - id: ruff-format + - id: ruff + name: ruff-isort + args: [ + "--select=I", + "--fix" + ] + - id: ruff + name: ruff-pyupgrade + args: [ + "--select=UP", + "--fix" + ] + - id: ruff + name: ruff-pydocstyle + args: [ + "--select=D", + "--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D401", + "--fix", + "--unsafe-fixes" + ] + stages: [pre-commit] + pass_filenames: true + - id: ruff + name: ruff-check + args: [ + "--select=F,PT,B,C4,T20,S,N", + "--ignore=T201,N812,B006,S101,S311,S607,S603", + "--fix" + ] + stages: [pre-commit] + pass_filenames: true + # C++ hooks + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v19.1.0 + hooks: + - id: clang-format + args: [--style=file] + # Spellcheck in comments and docs + - repo: https://github.com/codespell-project/codespell + rev: v2.3.0 + hooks: + - id: codespell + args: ['--write-changes', '--ignore-words-list=theses'] diff --git a/.releaserc b/.releaserc new file mode 100644 index 000000000..7bf05ecf4 --- /dev/null +++ b/.releaserc @@ -0,0 +1,8 @@ +{ + "branches": ["main"], + "plugins": [ + "@semantic-release/commit-analyzer", + "@semantic-release/release-notes-generator", + "@semantic-release/github" + ] +} diff --git a/README.md b/README.md index 01d2bbe1e..a7aac8a45 100644 --- a/README.md +++ b/README.md @@ -21,4 +21,3 @@ This repo contains software for operating UUVs, developed by students at NTNU. T * [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) * [Autonomous Navigation, Mapping, and Exploration for Underwater Robots](https://github.com/vortexntnu/Vortex-AUV/blob/documentation/top-level_readme/docs/master_theses/V%C3%A5ge%2C%20Utbjoe%2C%20Gjerden%20og%20Engebretsen%20(2019).pdf) - diff --git a/acoustics/acoustics_data_record/CMakeLists.txt b/acoustics/acoustics_data_record/CMakeLists.txt index ee711f38e..c99f84730 100644 --- a/acoustics/acoustics_data_record/CMakeLists.txt +++ b/acoustics/acoustics_data_record/CMakeLists.txt @@ -26,4 +26,4 @@ install(PROGRAMS DESTINATION lib/${PROJECT_NAME} ) -ament_package() \ No newline at end of file +ament_package() 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 c1c7b88cb..109c626a2 100755 --- a/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_lib.py +++ b/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_lib.py @@ -1,14 +1,14 @@ # Python Libraries -import time import csv +import time from datetime import datetime 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/" + 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" @@ -28,30 +28,51 @@ def __init__(self, ROS2_PACKAGE_DIRECTORY=""): "Position", ] - # Make new .csv file for loging blackbox data ---------- - with open(self.data_file_location, mode="w", newline="") as csv_file: + # 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) # 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], - ): - # Get current time in hours, minutes, seconds and miliseconds + 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. + + Parameters: + self (object): The instance of the class. + hydrophone1 (list, optional): Data from hydrophone 1. Defaults to [0]. + hydrophone2 (list, optional): Data from hydrophone 2. Defaults to [0]. + hydrophone3 (list, optional): Data from hydrophone 3. Defaults to [0]. + hydrophone4 (list, optional): Data from hydrophone 4. Defaults to [0]. + hydrophone5 (list, optional): Data from hydrophone 5. Defaults to [0]. + filter_response (list, optional): Filter response data. Defaults to [0]. + fft (list, optional): FFT data. Defaults to [0]. + peaks (list, optional): Peaks data. Defaults to [0]. + tdoa (list, optional): Time Difference of Arrival data. Defaults to [0.0]. + position (list, optional): Position data. Defaults to [0.0]. + + Writes the current time and provided data to a CSV file located at self.data_file_location. + """ + # Get current time in hours, minutes, seconds and milliseconds current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] # 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/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..9b1d57c8b 100755 --- a/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py +++ b/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py @@ -1,92 +1,90 @@ #!/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 +# Python Libraries import array -# Custom libraries +# ROS2 Libraries +import rclpy 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): - 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 = ( + 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 + tdoa_data_size = ( 5 # TDOA (Time Difference Of Arrival) has 5 hydrophones it has times for ) - positionDataSize = 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") # Initialize Subscribers ---------- # Start listening to Hydrophone data - self.subscriberHydrophone1 = self.create_subscription( + self.subscriber_hydrophone1 = self.create_subscription( Int32MultiArray, "/acoustics/hydrophone1", self.hydrophone1_callback, 5 ) - self.hydropone1Data = array.array("i", [0] * hydrophoneDataSize) + self.hydrophone1_data = array.array("i", [0] * hydrophone_data_size) - self.subscriberHydrophone2 = self.create_subscription( + self.subscriber_hydrophone2 = self.create_subscription( Int32MultiArray, "/acoustics/hydrophone2", self.hydrophone2_callback, 5 ) - self.hydropone2Data = array.array("i", [0] * hydrophoneDataSize) + self.hydrophone2_data = array.array("i", [0] * hydrophone_data_size) - self.subscriberHydrophone3 = self.create_subscription( + self.subscriber_hydrophone3 = self.create_subscription( Int32MultiArray, "/acoustics/hydrophone3", self.hydrophone3_callback, 5 ) - self.hydropone3Data = array.array("i", [0] * hydrophoneDataSize) + self.hydrophone3_data = array.array("i", [0] * hydrophone_data_size) - self.subscriberHydrophone4 = self.create_subscription( + self.subscriber_hydrophone4 = self.create_subscription( Int32MultiArray, "/acoustics/hydrophone4", self.hydrophone4_callback, 5 ) - self.hydropone4Data = array.array("i", [0] * hydrophoneDataSize) + self.hydrophone4_data = array.array("i", [0] * hydrophone_data_size) - self.subscriberHydrophone5 = self.create_subscription( + self.subscriber_hydrophone5 = self.create_subscription( Int32MultiArray, "/acoustics/hydrophone5", self.hydrophone5_callback, 5 ) - self.hydropone5Data = array.array("i", [0] * hydrophoneDataSize) + self.hydrophone5_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( + self.subscriber_fft = self.create_subscription( Int32MultiArray, "/acoustics/fft", self.fft_callback, 5 ) - self.FFTData = array.array("i", [0] * DSPDataSize) + self.fft_data = array.array("i", [0] * dsp_data_size) - self.subscriberPeaks = self.create_subscription( + self.subscriber_peaks = self.create_subscription( Int32MultiArray, "/acoustics/peaks", self.peaks_callback, 5 ) - self.peaksData = array.array("i", [0] * DSPDataSize) + 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( + self.subscriber_position_response = self.create_subscription( Float32MultiArray, "/acoustics/position", self.position_callback, 5 ) - self.positionData = array.array("f", [0.0] * positionDataSize) + self.position_data = array.array("f", [0.0] * position_data_size) # Initialize logger ---------- # Get package directory location @@ -101,24 +99,24 @@ def __init__(self): + "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 + 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 = ( + ) # 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 + 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" @@ -133,54 +131,120 @@ def __init__(self): "/acoustics/position [Float32MultiArray] \n" ) - # Callback methods for diffrenet topics - def hydrophone1_callback(self, msg): - self.hydropone1Data = msg.data + # Callback methods for different topics + def hydrophone1_callback(self, msg: Int32MultiArray) -> None: + """Callback method for hydrophone1 topic. + + Args: + msg (Int32MultiArray): Message containing hydrophone1 data. + """ + self.hydrophone1_data = msg.data + + def hydrophone2_callback(self, msg: Int32MultiArray) -> None: + """Callback method for hydrophone2 topic. - def hydrophone2_callback(self, msg): - self.hydropone2Data = msg.data + Args: + msg (Int32MultiArray): Message containing hydrophone2 data. + """ + self.hydrophone2_data = msg.data - def hydrophone3_callback(self, msg): - self.hydropone3Data = msg.data + def hydrophone3_callback(self, msg: Int32MultiArray) -> None: + """Callback method for hydrophone3 topic. - def hydrophone4_callback(self, msg): - self.hydropone4Data = msg.data + Args: + msg (Int32MultiArray): Message containing hydrophone3 data. + """ + self.hydrophone3_data = msg.data - def hydrophone5_callback(self, msg): - self.hydropone5Data = msg.data + def hydrophone4_callback(self, msg: Int32MultiArray) -> None: + """Callback method for hydrophone4 topic. - def filter_response_callback(self, msg): - self.filterResponseData = msg.data + Args: + msg (Int32MultiArray): Message containing hydrophone4 data. + """ + self.hydrophone4_data = msg.data - def fft_callback(self, msg): - self.FFTData = msg.data + def hydrophone5_callback(self, msg: Int32MultiArray) -> None: + """Callback method for hydrophone5 topic. - def peaks_callback(self, msg): - self.peaksData = msg.data + Args: + msg (Int32MultiArray): Message containing hydrophone5 data. + """ + self.hydrophone5_data = msg.data - def tdoa_callback(self, msg): - self.TDOAData = msg.data + def filter_response_callback(self, msg: Int32MultiArray) -> None: + """Callback method for filter_response topic. - def position_callback(self, msg): - self.positionData = msg.data + Args: + msg (Int32MultiArray): Message containing filter response data. + """ + self.filter_response_data = msg.data + + def fft_callback(self, msg: Int32MultiArray) -> None: + """Callback method for fft topic. + + Args: + msg (Int32MultiArray): Message containing FFT data. + """ + self.fft_data = msg.data + + def peaks_callback(self, msg: Int32MultiArray) -> None: + """Callback method for peaks topic. + + Args: + msg (Int32MultiArray): Message containing peaks data. + """ + self.peaks_data = msg.data + + def tdoa_callback(self, msg: Float32MultiArray) -> None: + """Callback method for time_difference_of_arrival topic. + + Args: + msg (Float32MultiArray): Message containing TDOA data. + """ + self.tdoa_data = msg.data + + def position_callback(self, msg: Float32MultiArray) -> None: + """Callback method for position topic. + + Args: + msg (Float32MultiArray): Message containing position data. + """ + self.position_data = msg.data # The logger that logs all the data - def logger(self): + def logger(self) -> None: + """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, - 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.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, + tdoa=self.tdoa_data, + position=self.position_data, ) -def main(): +def main() -> None: + """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() @@ -188,7 +252,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/launch/acoustics_data_record.launch.py b/acoustics/acoustics_data_record/launch/acoustics_data_record_launch.py similarity index 63% rename from acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py rename to acoustics/acoustics_data_record/launch/acoustics_data_record_launch.py index 517b0ee84..7e3a8dee5 100755 --- a/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py +++ b/acoustics/acoustics_data_record/launch/acoustics_data_record_launch.py @@ -1,10 +1,22 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory import os +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +from launch import LaunchDescription + + +def generate_launch_description() -> LaunchDescription: + """Generates a launch description for the acoustics_data_record node. + + 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. -def generate_launch_description(): + 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..b903e56a8 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -1,28 +1,26 @@ #!/usr/bin/env python3 # Libraries for file manipulation -import os -import sys -import ast import glob +import os + +import matplotlib.pyplot as plt # Libraries for handling data structures import pandas as pd -import numpy as np -import array -# Libraries for anmation -import matplotlib.animation as animation -import matplotlib.gridspec as gridspec -import matplotlib.pyplot as plt +# Libraries for animation +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 +# 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 basically 3 elements # Important variables for later processing of data SAMPLE_RATE = 430_000 # 430 kHz @@ -42,27 +40,27 @@ 2, 1, subplot_spec=outer_gs[1], height_ratios=[7, 3], hspace=0.3 ) -hydrophoneAxis = [None] * 5 +hydrophone_axis = [None] * 5 # Add subplots in the first column for hydrophone data for i in range(5): - hydrophoneAxis[i] = fig.add_subplot( - gs_hydrophone[i, 0], sharex=hydrophoneAxis[0] if i else None + hydrophone_axis[i] = fig.add_subplot( + gs_hydrophone[i, 0], sharex=hydrophone_axis[0] if i else None ) - hydrophoneAxis[i].label_outer() + hydrophone_axis[i].label_outer() fig.text(0.25, 0.965, "Hydrophone Data", ha="center") # Add subplots in the second column FFTAxis = fig.add_subplot(gs_dsp[0]) -filterAxis = fig.add_subplot(gs_dsp[1]) +filter_axis = fig.add_subplot(gs_dsp[1]) # Plot type so that the size is dynamic plt.tight_layout() # Select nice color pallet for graphs -colorSoftPurple = (168 / 255, 140 / 255, 220 / 255) -colorSoftBlue = (135 / 255, 206 / 255, 250 / 255) -colorSoftGreen = (122 / 255, 200 / 255, 122 / 255) +color_soft_purple = (168 / 255, 140 / 255, 220 / 255) +color_soft_blue = (135 / 255, 206 / 255, 250 / 255) +color_soft_green = (122 / 255, 200 / 255, 122 / 255) # .CSV Setup ================================================== # Get Directory of the .csv files @@ -70,105 +68,162 @@ ACOUSTICS_CSV_FILE_DIR = PACKAGE_DIR + "/acoustics_data" # List of all the acoustic files -acousticsCSVFiles = csv_files = glob.glob( +acoustics_csv_files = csv_files = glob.glob( ACOUSTICS_CSV_FILE_DIR + "/acoustics_data_" + "*.csv" ) # Get the latest csv file name for acoustics data -acousticsCSVFile = max(acousticsCSVFiles, key=os.path.getctime) +acoustics_csv_file = max(acoustics_csv_files, key=os.path.getctime) + +def convert_pandas_object_to_int_array(pandas_object: pd.Series) -> list: + """Convert a pandas object containing a string representation of an integer array to a list of integers. -def convertPandasObjectToIntArray(pandasObject): - pandasString = pandasObject.iloc[0].strip("array('i', ").rstrip(")") - pandasIntArray = [int(x.strip()) for x in pandasString.strip("[]").split(",")] + Args: + pandas_object (pandas.Series): A pandas Series object containing a string representation of an integer array. - return pandasIntArray + Returns: + list: A list of integers extracted from the pandas object. + """ + pandas_string = pandas_object.iloc[0].replace("array('i', ", "").replace(")", "") + pandas_int_array = [int(x.strip()) for x in pandas_string.strip("[]").split(",")] + return pandas_int_array -def convertPandasObjectToFloatArray(pandasObject): - pandasString = pandasObject.iloc[0].strip("array('f', ").rstrip(")") - pandasFloatArray = [float(x.strip()) for x in pandasString.strip("[]").split(",")] - return pandasFloatArray +def convert_pandas_object_to_float_array(pandas_object: pd.Series) -> list: + """Convert a pandas object containing a string representation of a float array to a list of floats. + Args: + pandas_object (pandas.Series): A pandas Series object containing a string representation of a float array. + + Returns: + list: A list of floats extracted from the pandas object. + """ + pandas_string = pandas_object.iloc[0].replace("array('f', ", "").replace(")", "") + pandas_float_array = [ + float(x.strip()) for x in pandas_string.strip("[]").split(",") + ] -def getAcousticsData(): + return pandas_float_array + + +def get_acoustics_data() -> list: + """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 - 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(acoustics_csv_file) + 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 + # 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 - 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"]) - except: - print("ERROR: Coulden't read acoustics data") + tdoa_data = convert_pandas_object_to_float_array(latest_acoustics_data["TDOA"]) + positon_data = convert_pandas_object_to_float_array( + latest_acoustics_data["Position"] + ) + 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 # 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 i in range(1, len(peaksData), 3): - if peaksData[i] < MAX_FREQUENCY_TO_SHOW: - tempAmplitude += [peaksData[i - 1]] - tempFrequency += [peaksData[i]] - - peaksAmplitudeData = tempAmplitude - peaksFrequencyData = tempFrequency - except: - print("ERROR processing DSP data") + 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}") # return processed data ---------- return [ @@ -177,64 +232,88 @@ 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, ] -def display_live_data(frame): +def display_live_data() -> None: + """Display live acoustics data by plotting hydrophone data, filter response, and FFT data. + + Retrieves the latest acoustics data and separates it into hydrophone data, unfiltered data, + filtered data, FFT amplitude and frequency data, and peak amplitude and frequency data. + 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() - - # 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 + acoustics_data = get_acoustics_data() + + # Set the latest acoustics data in appropriate variables + 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 i in range(5): - xHydrophone = list(range(len(hydrophoneData[i][::]))) - hydrophoneAxis[i].clear() - hydrophoneAxis[i].plot( - xHydrophone, - hydrophoneData[i], - label=f"Hydrophone {i + 1}", - color=colorSoftBlue, + for hydrophone_index in range(5): + x_hydrophone = list(range(len(hydrophone_data[hydrophone_index][::]))) + hydrophone_axis[hydrophone_index].clear() + hydrophone_axis[hydrophone_index].plot( + x_hydrophone, + hydrophone_data[hydrophone_index], + label=f"Hydrophone {hydrophone_index + 1}", + color=color_soft_blue, alpha=1, ) - hydrophoneAxis[i].legend(loc="upper right", fontsize="xx-small") + hydrophone_axis[hydrophone_index].legend(loc="upper right", fontsize="xx-small") # Plot Filter response - xRaw = list(range(len(unfilteredData))) - xFilter = list(range(len(filterData))) - 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 + x_raw = list(range(len(unfiltered_data))) + x_filter = list(range(len(filter_data))) + filter_axis.clear() + filter_axis.set_title("Filter response") + filter_axis.plot( + x_raw, unfiltered_data, label="Raw", color=color_soft_blue, alpha=0.5 + ) + filter_axis.plot( + x_filter, filter_data, label="Filter", color=color_soft_green, alpha=0.7 ) - filterAxis.legend(loc="upper right", fontsize="xx-small") + filter_axis.legend(loc="upper right", fontsize="xx-small") # Plot FFT data FFTAxis.clear() @@ -242,16 +321,16 @@ def display_live_data(frame): FFTAxis.set_xlabel("Frequency [Hz]") FFTAxis.set_ylabel("Amplitude") FFTAxis.bar( - FFTFrequencyData, - FFTAmplitudeData, + fft_frequency_data, + fft_amplitude_data, label="FFT", - color=colorSoftPurple, + color=color_soft_purple, alpha=1, width=500, ) FFTAxis.scatter( - peaksFrequencyData, - peaksAmplitudeData, + peaks_frequency_data, + peaks_amplitude_data, label="Peaks", color="red", alpha=0.7, @@ -262,7 +341,7 @@ def display_live_data(frame): 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/CMakeLists.txt b/acoustics/acoustics_interface/CMakeLists.txt index 82d472cbc..fecb507ea 100644 --- a/acoustics/acoustics_interface/CMakeLists.txt +++ b/acoustics/acoustics_interface/CMakeLists.txt @@ -28,4 +28,4 @@ install(PROGRAMS DESTINATION lib/${PROJECT_NAME} ) -ament_package() \ No newline at end of file +ament_package() diff --git a/acoustics/acoustics_interface/README b/acoustics/acoustics_interface/README index 25c1a8eac..73ef26b7c 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 -OBS!: Make sure to connect Teensy 4.1 MCU to the ethernet \ No newline at end of file +Interface to communicate with acoustics PCB and Teensy 4.1 MCU +OBS!: Make sure to connect Teensy 4.1 MCU to the ethernet diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index aec0ded6e..a9c1c9817 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -1,17 +1,13 @@ # Setting up libraries -import os -import sys -from socket import * -import netifaces as ni -from enum import Enum import errno import time +from socket import AF_INET, SOCK_DGRAM, socket class TeensyCommunicationUDP: - """ - This class is responsible for the RPI side of teensy-RPI UDP communication. It is - implemented with a singleton pattern for convenience. + """This class is responsible for the RPI side of teensy-RPI UDP communication. + + It is implemented with a singleton pattern for convenience. Note: Private members are denoted by _member_name @@ -26,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: -------- @@ -59,9 +55,9 @@ class TeensyCommunicationUDP: _INITIALIZATION_MESSAGE = "HELLO :D" # This is a message only sent once to establish 2 way communication between Teensy and client - _clientSocket = socket(AF_INET, SOCK_DGRAM) + _client_socket = socket(AF_INET, SOCK_DGRAM) - _timeoutMax = 10 + _timeout_max = 10 _data_string = "" _data_target = "" acoustics_data = { @@ -78,57 +74,54 @@ class TeensyCommunicationUDP: } @classmethod - def init_communication(cls, frequenciesOfInterest: list[tuple[int, int]]) -> None: - """ - Sets up communication with teensy + 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 + 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() # Socket setup - cls._clientSocket.settimeout(cls._TIMEOUT) - cls._clientSocket.bind((cls.MY_IP, cls._MY_PORT)) - cls._clientSocket.setblocking(False) + cls._client_socket.settimeout(cls._TIMEOUT) + cls._client_socket.bind((cls.MY_IP, cls._MY_PORT)) + cls._client_socket.setblocking(False) cls._send_acknowledge_signal() - 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._timeout_max: 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: - """ - Gets data from teensy and stores it in `acoustics_data` - """ + """Gets data from teensy and stores it in `acoustics_data`.""" i = 0 while True: data = cls._get_raw_data() - if data == 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() @@ -144,15 +137,13 @@ def fetch_data(cls) -> None: @classmethod def _write_to_target(cls) -> None: - """ - Writes to the current target in `acoustics_data` and clears the data string - """ - if cls._data_target == "TDOA" or cls._data_target == "LOCATION": + """Writes to the current target in `acoustics_data` and clears the data string.""" + if cls._data_target in {"TDOA", "LOCATION"}: data = cls._parse_data_string(is_float=True) else: data = cls._parse_data_string(is_float=False) - if data == None: + if data is None: cls._data_string = "" return @@ -162,17 +153,16 @@ def _write_to_target(cls) -> None: @classmethod def _get_raw_data(cls) -> str | None: - """ - Gets a message from teensy + """Gets a message from teensy. Returns: The message in the UDP buffer if there is one """ try: - rec_data, _ = cls._clientSocket.recvfrom(cls._MAX_PACKAGE_SIZE_RECEIVED) - messageReceived = rec_data.decode() - return messageReceived - except error as e: # `error` is really `socket.error` + rec_data, _ = cls._client_socket.recvfrom(cls._MAX_PACKAGE_SIZE_RECEIVED) + message_received = rec_data.decode() + return message_received + except OSError as e: # `error` is really `socket.error` if e.errno == errno.EWOULDBLOCK: pass else: @@ -180,8 +170,7 @@ def _get_raw_data(cls) -> str | None: @classmethod def _parse_data_string(cls, is_float: bool) -> list[float] | list[int] | None: - """ - Converts _data_string to a list + """Converts _data_string to a list. Parameters: is_float (bool): whether _data_string should be seen as a list of floats or ints @@ -196,8 +185,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}") @@ -206,39 +194,35 @@ def _parse_data_string(cls, is_float: bool) -> list[float] | list[int] | None: # https://stackoverflow.com/questions/166506/finding-local-ip-addresses-using-pythons-stdlib @classmethod def _get_ip(cls) -> None: - """ - Gets the device's IP address - """ + """Gets the device's IP address.""" s = socket(AF_INET, SOCK_DGRAM) s.settimeout(0) try: # 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: - """ - Sends "HELLO :D to teensy - """ + """Sends "HELLO :D to teensy.""" try: - cls._clientSocket.sendto(cls._INITIALIZATION_MESSAGE.encode(), cls._address) - print("DEBUGING: Sent acknowledge package") + cls._client_socket.sendto( + cls._INITIALIZATION_MESSAGE.encode(), cls._address + ) + print("DEBUGGING: Sent acknowledge package") except Exception as e: print("Error from send_acknowledge_signal") print(e) - pass @classmethod def _check_if_available(cls) -> None: - """ - Checks if READY has been received + """Checks if READY has been received. Note: The while loop here may not be necessary, it is just there to make absolutely sure that *all* the data in the UDP buffer is read out when waiting for ready signal, to avoid strange bugs @@ -249,7 +233,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 @@ -268,10 +252,9 @@ def _check_if_available(cls) -> None: @classmethod def _send_frequencies_of_interest( - cls, frequenciesOfInterest: list[tuple[float, float]] + cls, frequencies_of_interest: list[tuple[float, float]] ) -> None: - """ - Sends the list of frequencies with variance to teensy + """Sends the list of frequencies with variance to teensy. Parameters: frequenciesOfInterest (list[tuple[float, float]]): The list of frequencies w/ variance @@ -279,14 +262,14 @@ def _send_frequencies_of_interest( try: # Format (CSV): xxx,x,xx,x...,x (frequency list comes first, then variances) assert ( - len(frequenciesOfInterest) == 10 + 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); - cls._clientSocket.sendto(frequency_variance_msg.encode(), cls._address) - except: - print("Couldn't send Frequency data") + cls._client_socket.sendto(frequency_variance_msg.encode(), cls._address) + except Exception as e: + print(f"Unexpected error while sending frequency data: {e}") diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py index f474c11db..87b2d31d7 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py @@ -1,15 +1,15 @@ #!/usr/bin/python3 import rclpy -from rclpy.node import Node import rclpy.logging -from std_msgs.msg import Int32MultiArray, Float32MultiArray +from rclpy.node import Node +from std_msgs.msg import Float32MultiArray, Int32MultiArray + from acoustics_interface.acoustics_interface_lib import TeensyCommunicationUDP class AcousticsInterfaceNode(Node): - """ - Publishes Acoustics data to ROS2 + """Publishes Acoustics data to ROS2. Methods: data_update() -> None: @@ -19,9 +19,7 @@ class AcousticsInterfaceNode(Node): """ def __init__(self) -> None: - """ - Sets up acoustics logging and publishers, also sets up teensy communication - """ + """Sets up acoustics logging and publishers, also sets up teensy communication.""" super().__init__("acoustics_interface") rclpy.logging.initialize() @@ -60,13 +58,13 @@ def __init__(self) -> None: # 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 = ( + ) # 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_LOGING_RATE + 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( @@ -77,35 +75,40 @@ def __init__(self) -> None: # 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 = ( + 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 = [] + 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], ) ] - # 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...") - TeensyCommunicationUDP.init_communication(frequenciesOfInterest) + 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: + """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: - """Publishes to topics""" + """Publishes to topics.""" self._hydrophone_1_publisher.publish( Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_1"]) ) @@ -142,7 +145,16 @@ def data_publisher(self) -> None: ) -def main(args=None): +def main(args: list = None) -> 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() @@ -151,7 +163,6 @@ def main(args=None): node.destroy_node() rclpy.shutdown() - pass if __name__ == "__main__": diff --git a/acoustics/acoustics_interface/launch/acoustics_interface_launch.py b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py index 92e3b0b20..b040b403b 100755 --- a/acoustics/acoustics_interface/launch/acoustics_interface_launch.py +++ b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py @@ -1,10 +1,22 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory import os +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +from launch import LaunchDescription + + +def generate_launch_description() -> LaunchDescription: + """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. -def generate_launch_description(): + 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/CMakeLists.txt b/auv_setup/CMakeLists.txt index ae1fe2d7e..d91c7a44a 100644 --- a/auv_setup/CMakeLists.txt +++ b/auv_setup/CMakeLists.txt @@ -7,13 +7,6 @@ endif() find_package(ament_cmake REQUIRED) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - # Install launch files. install(DIRECTORY config diff --git a/auv_setup/config/robots/beluga.yaml b/auv_setup/config/robots/beluga.yaml index 0895f2ed2..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] # Weigths 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,20 +237,20 @@ 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 - loger: + logger: interval: 10 # seconds 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 9cbc3b4ef..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' -torpedos: # 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 f98dce738..eafcccfc6 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,109 @@ 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, 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 - - 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 - - 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 - - blackbox: - data_logging_rate: 5.0 # [logings/second], Recomended: 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 - [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 # [logings/second], Recomended: 1.0 logings per second + 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, + ] + 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' diff --git a/auv_setup/launch/orca.launch.py b/auv_setup/launch/orca.launch.py index 2f56245a1..12b3b0bc4 100755 --- a/auv_setup/launch/orca.launch.py +++ b/auv_setup/launch/orca.launch.py @@ -1,12 +1,23 @@ 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(): +def generate_launch_description() -> LaunchDescription: + """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..fff86a3b2 100755 --- a/auv_setup/launch/topside.launch.py +++ b/auv_setup/launch/topside.launch.py @@ -1,12 +1,23 @@ 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(): +def generate_launch_description() -> LaunchDescription: + """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/README b/mission/LCD/README index 5cdd4ca7c..8af8e8705 100644 --- a/mission/LCD/README +++ b/mission/LCD/README @@ -1,8 +1,8 @@ # 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: +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: -./vortex-auv/add_service_files_to_bootup_sequence.sh \ No newline at end of file +and execute script that adds our LCD screen srit into the boot-up sequence: +./vortex-auv/add_service_files_to_bootup_sequence.sh diff --git a/mission/LCD/sources/IP_lib.py b/mission/LCD/sources/IP_lib.py deleted file mode 100644 index a71d029c5..000000000 --- a/mission/LCD/sources/IP_lib.py +++ /dev/null @@ -1,14 +0,0 @@ -#!/usr/bin/python3 -# Python Libraries -import subprocess - - -class IPDriver: - def __init__(self): - self.cmd = "hostname -I | cut -d' ' -f1" - - def get_IP(self): - IP_bytes = subprocess.check_output(self.cmd, shell=True) - IP_str = IP_bytes.decode("utf-8") - - return IP_str diff --git a/mission/LCD/sources/LCD.py b/mission/LCD/sources/LCD.py deleted file mode 100644 index 41f6f7fa9..000000000 --- a/mission/LCD/sources/LCD.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/python3 -# Python Libraries -from time import sleep - -# 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 - -# Initialize all necesarry drivers/libraries for the display and functionality -LCD = LCDScreenDriver() -IP = IPDriver() -PSM = PowerSenseModule() -Pressure = PressureSensor() -Temperature = TemperatureSensor() - - -# Formating function for nices LCD screen layout -def format_line(value: str, unit: str): - spacesAvailable = 16 - valueLenght = len(value) - unitLenght = ( - len(unit) + 1 - ) # +1 to make sure there is spacing between value and unit - - emptySpaceLenght = spacesAvailable - (valueLenght + unitLenght) - if emptySpaceLenght < 0: - emptySpaceLenght = 0 - - formatedString = value[0 : (spacesAvailable - unitLenght)] - formatedString += " " * emptySpaceLenght - formatedString += " " + unit - - return formatedString - - -# Fancy animation at the start -LCD.fancy_animation(animation_speed=3.0) - -# 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) - - # 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) - - # 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) diff --git a/mission/LCD/sources/ip_lib.py b/mission/LCD/sources/ip_lib.py new file mode 100644 index 000000000..71eb15dc1 --- /dev/null +++ b/mission/LCD/sources/ip_lib.py @@ -0,0 +1,28 @@ +#!/usr/bin/python3 +# Python Libraries +import subprocess + + +class IPDriver: + def __init__(self) -> None: + # Store command as a list of arguments + self.cmd = ["hostname", "-I"] + + def get_ip(self) -> str: + """Executes a shell command to retrieve the IP address. + + Returns: + str: The IP address as a string. + """ + try: + # Run the command without shell=True + ip_bytes = subprocess.check_output(self.cmd, stderr=subprocess.STDOUT) + ip_str = ip_bytes.decode("utf-8").strip() + + # Split by space and get the first IP + return ip_str.split()[0] + + except subprocess.CalledProcessError as e: + # Handle the error appropriately (e.g., log it or raise an exception) + print(f"Failed to retrieve IP address: {e}") + return "" diff --git a/mission/LCD/sources/lcd.py b/mission/LCD/sources/lcd.py new file mode 100644 index 000000000..1af56d0fe --- /dev/null +++ b/mission/LCD/sources/lcd.py @@ -0,0 +1,79 @@ +#!/usr/bin/python3 + +# Python Libraries +from time import sleep + +from ip_lib import IPDriver + +# Custom Libraries +from lcd_lib import LCDScreenDriver +from power_sense_module_lib import PowerSenseModule +from pressure_sensor_lib import PressureSensor +from temperature_sensor_lib import TemperatureSensor + +# Initialize all necessary drivers/libraries for the display and functionality +LCD = LCDScreenDriver() +IP = IPDriver() +PSM = PowerSenseModule() +Pressure = PressureSensor() +Temperature = TemperatureSensor() + + +# Formatting function for nices LCD screen layout +def format_line(value: str, unit: str) -> str: + """Formats a string to fit within a 16-character display, appending a unit with spacing. + + 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. + """ + spaces_available = 16 + value_length = len(value) + unit_length = ( + len(unit) + 1 + ) # +1 to make sure there is spacing between value and unit + + empty_space_length = spaces_available - (value_length + unit_length) + empty_space_length = max(empty_space_length, 0) + + formated_string = value[0 : (spaces_available - unit_length)] + formated_string += " " * empty_space_length + formated_string += " " + unit + + return formated_string + + +# Fancy animation at the start +LCD.fancy_animation(animation_speed=3.0) + +# Display information on the LDC Screen +while True: + # IP ---------- + 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 ---------- + 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 ---------- + 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 similarity index 53% rename from mission/LCD/sources/LCD_lib.py rename to mission/LCD/sources/lcd_lib.py index 2d28466cd..70b8e5d6e 100644 --- a/mission/LCD/sources/LCD_lib.py +++ b/mission/LCD/sources/lcd_lib.py @@ -7,11 +7,11 @@ class LCDScreenDriver: - def __init__(self): + def __init__(self) -> None: # Initialize LCD Screen lcd_i2c_address = 0x27 - self._LCD = CharLCD( + self._lcd = CharLCD( i2c_expander="PCF8574", address=lcd_i2c_address, port=1, @@ -23,19 +23,47 @@ def __init__(self): backlight_enabled=True, ) - def write_to_screen(self, line1: str = "", line2: str = ""): - self._LCD.clear() + def write_to_screen(self, line1: str = "", line2: str = "") -> None: + """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 - 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) + + 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. + + 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. - self._LCD.write_string(line1 + "\r\n") - self._LCD.write_string(line2) + Custom characters used: + - Pac-Man with mouth open (left and right facing) + - Pac-Man with mouth closed + - Ghost - def fancy_animation(self, animation_speed=0.4): - # Calculate the apropriate animation speed + The animation is displayed in two rows of the LCD screen. + """ + # Calculate the appropriate animation speed animation_speed = 1 / animation_speed # Custom characters ---------- @@ -72,55 +100,55 @@ 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() + 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 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() + 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 = ( @@ -128,10 +156,10 @@ def fancy_animation(self, animation_speed=0.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 fd9400e5a..3713bd1ad 100755 --- a/mission/LCD/sources/power_sense_module_lib.py +++ b/mission/LCD/sources/power_sense_module_lib.py @@ -1,11 +1,12 @@ -# Libraies for Power Sense Module +# Libraries for Power Sense Module import time + import smbus from MCP342x import MCP342x 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 @@ -16,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 @@ -31,12 +32,22 @@ 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 - def get_voltage(self): + def get_voltage(self) -> float: + """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 = ( @@ -47,7 +58,15 @@ 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. + + 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..7a72c6617 100755 --- a/mission/LCD/sources/pressure_sensor_lib.py +++ b/mission/LCD/sources/pressure_sensor_lib.py @@ -2,22 +2,23 @@ # 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: - def __init__(self): + def __init__(self) -> None: # 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, @@ -28,7 +29,12 @@ def __init__(self): time.sleep(1) - def get_pressure(self): + def get_pressure(self) -> float: + """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 807a672d1..14a0b3942 100755 --- a/mission/LCD/sources/temperature_sensor_lib.py +++ b/mission/LCD/sources/temperature_sensor_lib.py @@ -1,9 +1,9 @@ #!/usr/bin/python3 -""" -! NOTE: -! 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 +"""! NOTE. + +! For now we don't have a external sensor to measure internal temperature +! Instead we just use Internal Computer temperature sensor to gaugue temperature of the environment approximately +! In the future someone should implement a external temperature sensor for measuting a more accurate state of the temperatuer on the inside of the AUV. """ # Python Libraries @@ -11,17 +11,26 @@ 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. + + 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( ["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/LCD/startup_scripts/LCD.service b/mission/LCD/startup_scripts/LCD.service index 206c5c38a..e29830516 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,8 +14,8 @@ 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] -WantedBy=multi-user.target \ No newline at end of file +WantedBy=multi-user.target diff --git a/mission/blackbox/README b/mission/blackbox/README index 4cb923305..bbe3caf0b 100644 --- a/mission/blackbox/README +++ b/mission/blackbox/README @@ -9,11 +9,11 @@ Logs data of all the important topics such as: /pwm -## Service file bootup +## Service file boot-up -To start the blackbox loging automaticaly 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 ``` -Enjoy :) \ No newline at end of file +Enjoy :) diff --git a/mission/blackbox/blackbox/blackbox_log_data.py b/mission/blackbox/blackbox/blackbox_log_data.py index ac6976234..76f84e618 100755 --- a/mission/blackbox/blackbox/blackbox_log_data.py +++ b/mission/blackbox/blackbox/blackbox_log_data.py @@ -1,18 +1,18 @@ #!/usr/bin/env python3 # Python Libraries +import csv import os import re import time -import csv from datetime import datetime, timedelta 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/" + 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" @@ -43,19 +43,39 @@ 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 ---------- - with open(self.data_file_location, mode="w", newline="") as csv_file: + # 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) # 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) + self, max_file_age_in_days: int = 7, max_size_kb: int = 3_000_000 + ) -> None: + """Manages CSV files in the blackbox data directory by deleting old files and ensuring the total size does not exceed a specified limit. + + Args: + max_file_age_in_days (int, optional): The maximum age of files in days before they are deleted. Defaults to 7 days. + max_size_kb (int, optional): The maximum total size of all CSV files in kilobytes before the oldest files are deleted. + + 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) @@ -144,32 +164,39 @@ def manage_csv_files( # 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, - ): - # Get current time in hours, minutes, seconds and miliseconds + 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. + + This method appends a new row to the CSV file specified by `self.data_file_location`. + The row contains the current time and the provided data values. + """ + # Get current time in hours, minutes, seconds and milliseconds current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] # Write to .csv file - with open(self.data_file_location, mode="a", newline="") 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 077fd0f7c..ace7e6bb4 100755 --- a/mission/blackbox/blackbox/blackbox_node.py +++ b/mission/blackbox/blackbox/blackbox_node.py @@ -1,21 +1,23 @@ #!/usr/bin/env python3 # ROS2 Libraries -import rclpy import array -from rclpy.node import Node + +import rclpy from ament_index_python.packages import get_package_share_directory +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): - def __init__(self): + def __init__(self) -> None: # Start the ROS2 Node ---------- super().__init__("blackbox_node") @@ -62,24 +64,24 @@ def __init__(self): 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 + 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 = ( + ) # 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_LOGING_RATE + 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" @@ -91,25 +93,94 @@ 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. + + 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): + def psm_voltage_callback(self, msg: Float32) -> None: + """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): + def pressure_callback(self, msg: Float32) -> None: + """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): + def temperature_callback(self, msg: Float32) -> None: + """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): + def thruster_forces_callback(self, msg: ThrusterForces) -> None: + """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): + def pwm_callback(self, msg: Int16MultiArray) -> None: + """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): + def logger(self) -> None: + """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, @@ -134,7 +205,16 @@ def logger(self): ) -def main(args=None): +def main(args: list = None) -> 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/blackbox_data/.gitignore b/mission/blackbox/blackbox_data/.gitignore index f88f98717..0c7646f66 100644 --- a/mission/blackbox/blackbox_data/.gitignore +++ b/mission/blackbox/blackbox_data/.gitignore @@ -1 +1 @@ -blackbox_data_* \ No newline at end of file +blackbox_data_* diff --git a/mission/blackbox/launch/blackbox.launch.py b/mission/blackbox/launch/blackbox.launch.py index 3f675d368..02f8844a0 100644 --- a/mission/blackbox/launch/blackbox.launch.py +++ b/mission/blackbox/launch/blackbox.launch.py @@ -1,10 +1,20 @@ +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(): +def generate_launch_description() -> LaunchDescription: + """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/blackbox/package.xml b/mission/blackbox/package.xml index 35d8c1a7d..b7e3b2578 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 later vortex MIT diff --git a/mission/blackbox/startup_scripts/blackbox.service b/mission/blackbox/startup_scripts/blackbox.service index 62d30e3dd..aedb8eba7 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 @@ -14,4 +14,4 @@ Restart=no RestartSec=2 [Install] -WantedBy=multi-user.target \ No newline at end of file +WantedBy=multi-user.target diff --git a/mission/blackbox/startup_scripts/blackbox.sh b/mission/blackbox/startup_scripts/blackbox.sh index d70454ebe..9c5b307c3 100644 --- a/mission/blackbox/startup_scripts/blackbox.sh +++ b/mission/blackbox/startup_scripts/blackbox.sh @@ -2,6 +2,6 @@ SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" cd $SCRIPT_DIR -cd ../../../../.. # Go back to workspace +cd ../../../../.. # Go back to workspace source install/setup.bash ros2 launch blackbox blackbox.launch.py diff --git a/mission/gui_auv/auv_gui/__init__.py b/mission/gui_auv/auv_gui/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mission/gui_auv/auv_gui/auv_gui.py b/mission/gui_auv/auv_gui/auv_gui.py new file mode 100644 index 000000000..9257c691b --- /dev/null +++ b/mission/gui_auv/auv_gui/auv_gui.py @@ -0,0 +1,317 @@ +import math +import sys +from threading import Thread +from typing import List, Optional + +import matplotlib.pyplot as plt +import numpy as np +import rclpy +from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas +from nav_msgs.msg import Odometry +from PyQt6.QtCore import QTimer +from PyQt6.QtWidgets import ( + QApplication, + QGridLayout, + QLabel, + QMainWindow, + QTabWidget, + QVBoxLayout, + QWidget, +) +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from std_msgs.msg import Float32 + +# --- Quaternion to Euler angles --- + + +def quaternion_to_euler(x: float, y: float, z: float, w: float) -> List[float]: + """Convert a quaternion to Euler angles (roll, pitch, yaw). + + Args: + x (float): The x component of the quaternion. + y (float): The y component of the quaternion. + z (float): The z component of the quaternion. + w (float): The w component of the quaternion. + + Returns: + List[float]: A list of Euler angles [roll, pitch, yaw]. + """ + # Roll (x-axis rotation) + sinr_cosp = 2 * (w * x + y * z) + cosr_cosp = 1 - 2 * (x * x + y * y) + roll = np.degrees(math.atan2(sinr_cosp, cosr_cosp)) + + # Pitch (y-axis rotation) + sinp = 2 * (w * y - z * x) + pitch = np.degrees(math.asin(sinp)) + + # Yaw (z-axis rotation) + siny_cosp = 2 * (w * z + x * y) + cosy_cosp = 1 - 2 * (y * y + z * z) + yaw = np.degrees(math.atan2(siny_cosp, cosy_cosp)) + + return [roll, pitch, yaw] + + +# --- GUI Node --- + + +class GuiNode(Node): + """ROS2 Node that subscribes to odometry data and stores x, y positions.""" + + def __init__(self) -> None: + """Initialize the GuiNode and set up the odometry subscriber.""" + super().__init__("auv_gui_node") + + # ROS2 parameters + self.declare_parameter("odom_topic", "/nucleus/odom") + self.declare_parameter("current_topic", "/auv/power_sense_module/current") + self.declare_parameter("voltage_topic", "/auv/power_sense_module/voltage") + self.declare_parameter("temperature_topic", "/auv/temperature") + self.declare_parameter("pressure_topic", "/auv/pressure") + self.declare_parameter("history_length", 30) + + odom_topic = self.get_parameter("odom_topic").get_parameter_value().string_value + current_topic = ( + self.get_parameter("current_topic").get_parameter_value().string_value + ) + voltage_topic = ( + self.get_parameter("voltage_topic").get_parameter_value().string_value + ) + temperature_topic = ( + self.get_parameter("temperature_topic").get_parameter_value().string_value + ) + pressure_topic = ( + self.get_parameter("pressure_topic").get_parameter_value().string_value + ) + + # Subscriber to the /nucleus/odom topic + self.subscription = self.create_subscription( + Odometry, odom_topic, self.odom_callback, 10 + ) + + # Variables to store odometry data + self.xpos_data: List[float] = [] # x position + self.ypos_data: List[float] = [] # y position + self.zpos_data: List[float] = [] # z position + + self.w_data: List[float] = [] # w component of the quaternion + self.x_data: List[float] = [] # x component of the quaternion + self.y_data: List[float] = [] # y component of the quaternion + self.z_data: List[float] = [] # z component of the quaternion + + self.roll: Optional[float] = None + self.pitch: Optional[float] = None + self.yaw: Optional[float] = None + + # Subscribe to internal status topics + self.current_subscriber = self.create_subscription( + Float32, current_topic, self.current_callback, 5 + ) + self.voltage_subscriber = self.create_subscription( + Float32, voltage_topic, self.voltage_callback, 5 + ) + self.temperature_subscriber = self.create_subscription( + Float32, temperature_topic, self.temperature_callback, 5 + ) + self.pressure_subscriber = self.create_subscription( + Float32, pressure_topic, self.pressure_callback, 5 + ) + + # Variables for internal status + self.current = 0.0 + self.voltage = 0.0 + self.temperature = 0.0 + self.pressure = 0.0 + + # --- Callback functions --- + + def odom_callback(self, msg: Odometry) -> None: + """Callback function that is triggered when an odometry message is received.""" + # Extract x, y, z positions from the odometry message + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + z = -(msg.pose.pose.position.z) + self.xpos_data.append(x) + self.ypos_data.append(y) + self.zpos_data.append(z) + + # Extract the quaternion components from the odometry message + w = msg.pose.pose.orientation.w + x = msg.pose.pose.orientation.x + y = msg.pose.pose.orientation.y + z = msg.pose.pose.orientation.z + self.roll, self.pitch, self.yaw = quaternion_to_euler(x, y, z, w) + + # Limit the stored data for real-time plotting (avoid memory overflow) + max_data_points = ( + self.get_parameter("history_length").get_parameter_value().integer_value + ) + if len(self.x_data) > max_data_points: + self.xpos_data.pop(0) + self.ypos_data.pop(0) + self.zpos_data.pop(0) + + def current_callback(self, msg: Float32) -> None: + """Callback function that is triggered when a current message is received.""" + self.current = msg.data + + def voltage_callback(self, msg: Float32) -> None: + """Callback function that is triggered when a voltage message is received.""" + self.voltage = msg.data + + def temperature_callback(self, msg: Float32) -> None: + """Callback function that is triggered when a temperature message is received.""" + self.temperature = msg.data + + def pressure_callback(self, msg: Float32) -> None: + """Callback function that is triggered when a pressure message is received.""" + self.pressure = msg.data + + +# --- Plotting --- + + +class PlotCanvas(FigureCanvas): + """A canvas widget for plotting odometry data using matplotlib.""" + + def __init__(self, gui_node: GuiNode, parent: Optional[QWidget] = None) -> None: + """Initialize the PlotCanvas.""" + # Set up the 3D plot + self.gui_node = gui_node + self.fig = plt.figure() + self.ax = self.fig.add_subplot(111, projection='3d') + + # Initialize a red dot for the current position + (self.current_position_dot,) = self.ax.plot([], [], [], 'ro') + + super().__init__(self.fig) + self.setParent(parent) + + # Set labels and title for the 3D plot + self.ax.set_xlabel("X") + self.ax.set_ylabel("Y") + self.ax.set_zlabel("Z") + self.ax.set_title("Position") + + # Initialize data lists for 3D plot + self.x_data: List[float] = [] + self.y_data: List[float] = [] + self.z_data: List[float] = [] + (self.line,) = self.ax.plot([], [], [], 'b-') + + def update_plot( + self, x_data: List[float], y_data: List[float], z_data: List[float] + ) -> None: + """Update the 3D plot with the latest odometry data.""" + # Convert lists to numpy arrays to ensure compatibility with the plot functions + x_data = np.array(x_data, dtype=float) + y_data = np.array(y_data, dtype=float) + z_data = np.array(z_data, dtype=float) + + # Check if the arrays are non-empty before updating the plot + if len(x_data) > 0 and len(y_data) > 0 and len(z_data) > 0: + self.line.set_data(x_data, y_data) + self.line.set_3d_properties(z_data) + + # Update the current position dot + self.current_position_dot.set_data(x_data[-1:], y_data[-1:]) + self.current_position_dot.set_3d_properties(z_data[-1:]) + + # Update the limits for the 3D plot around the latest data point + x_latest = x_data[-1] + y_latest = y_data[-1] + z_latest = z_data[-1] + margin = 2.5 # Define a margin around the latest point + + self.ax.set_xlim(x_latest - margin, x_latest + margin) + self.ax.set_ylim(y_latest - margin, y_latest + margin) + self.ax.set_zlim(z_latest - margin, z_latest + margin) + + self.fig.canvas.draw() + self.fig.canvas.flush_events() + + +def run_ros_node(ros_node: GuiNode, executor: MultiThreadedExecutor) -> None: + """Run the ROS2 node in a separate thread using a MultiThreadedExecutor.""" + rclpy.spin(ros_node, executor) + + +def main(args: Optional[List[str]] = None) -> None: + """The main function to initialize ROS2 and the GUI application.""" + rclpy.init(args=args) + ros_node = GuiNode() + executor = MultiThreadedExecutor() + + # Run ROS in a separate thread + ros_thread = Thread(target=run_ros_node, args=(ros_node, executor), daemon=True) + ros_thread.start() + + # Setup the PyQt5 application and window + app = QApplication(sys.argv) + gui = QMainWindow() + gui.setWindowTitle("Vortex GUI") + gui.setGeometry(100, 100, 600, 400) + + # Create the tab widget + tabs = QTabWidget() + tabs.setTabPosition(QTabWidget.TabPosition.North) + tabs.setMovable(True) + + # --- Position Tab --- + position_widget = QWidget() + layout = QGridLayout(position_widget) # grid layout + + plot_canvas = PlotCanvas(ros_node, position_widget) + layout.addWidget(plot_canvas, 0, 0) + + current_pos = QLabel(parent=position_widget) + layout.addWidget(current_pos, 0, 1) + + tabs.addTab(position_widget, "Position") + + # --- Internal Status Tab --- + internal_widget = QWidget() + internal_layout = QVBoxLayout(internal_widget) + + internal_status_label = QLabel(parent=internal_widget) + internal_layout.addWidget(internal_status_label) + tabs.addTab(internal_widget, "Internal") + + gui.setCentralWidget(tabs) + gui.showMaximized() + + # Use a QTimer to update plot, current position, and internal status in the main thread + def update_gui() -> None: + plot_canvas.update_plot( + ros_node.xpos_data, ros_node.ypos_data, ros_node.zpos_data + ) + if len(ros_node.xpos_data) > 0 and ros_node.roll is not None: + position_text = f"Current Position:\nX: {ros_node.xpos_data[-1]:.2f}\nY: {ros_node.ypos_data[-1]:.2f}\nZ: {ros_node.zpos_data[-1]:.2f}" + orientation_text = f"Current Orientation:\nRoll: {ros_node.roll:.2f}\nPitch: {ros_node.pitch:.2f}\nYaw: {ros_node.yaw:.2f}" + current_pos.setText(position_text + "\n\n\n" + orientation_text) + + # Update internal status + internal_status_label.setText( + f"Internal Status:\n" + f"Current: {ros_node.current:.2f}\n" + f"Voltage: {ros_node.voltage:.2f}\n" + f"Temperature: {ros_node.temperature:.2f}\n" + f"Pressure: {ros_node.pressure:.2f}" + ) + + # Set up the timer to call update_gui every 100ms + timer = QTimer() + timer.timeout.connect(update_gui) + timer.start(100) + + app.exec() + + ros_node.destroy_node() + rclpy.shutdown() + sys.exit() + + +if __name__ == '__main__': + main() diff --git a/mission/gui_auv/config/gui_params.yaml b/mission/gui_auv/config/gui_params.yaml new file mode 100644 index 000000000..a490595f0 --- /dev/null +++ b/mission/gui_auv/config/gui_params.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + gui_auv: + odom_topic: "/nucleus/odom" + current_topic: "/auv/power_sense_module/current" + voltage_topic: "/auv/power_sense_module/voltage" + temperature_topic: "/auv/temperature" + pressure_topic: "/auv/pressure" + history_length: 30 # seconds diff --git a/mission/gui_auv/package.xml b/mission/gui_auv/package.xml new file mode 100644 index 000000000..dd89ae4cd --- /dev/null +++ b/mission/gui_auv/package.xml @@ -0,0 +1,19 @@ + + + + auv-gui + 0.0.0 + AUV GUI for ROS2 + sondre + MIT + + python3-pytest + + rclpy + geometry_msgs + sensor_msgs + + + ament_python + + diff --git a/mission/gui_auv/setup.py b/mission/gui_auv/setup.py new file mode 100644 index 000000000..de9acfda1 --- /dev/null +++ b/mission/gui_auv/setup.py @@ -0,0 +1,21 @@ +from setuptools import setup + +PACKAGE_NAME = 'auv_gui' + +setup( + name=PACKAGE_NAME, + version='0.0.0', + packages=[PACKAGE_NAME], + install_requires=['setuptools'], + zip_safe=True, + maintainer='sondre', + maintainer_email='sondre95556888@gmail.com', + description='AUV GUI for ROS2', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'auv-gui = auv_gui.auv_gui:main', + ], + }, +) diff --git a/mission/internal_status_auv/README.md b/mission/internal_status_auv/README.md index 491c159d2..d231d39ee 100644 --- a/mission/internal_status_auv/README.md +++ b/mission/internal_status_auv/README.md @@ -1,4 +1,4 @@ -# Internal Status +# Internal Status This package contains a library which allows us to get current and voltage values from the Power Sense Module (PSM), and a ros node which publishes this data to the whole ROS environment. @@ -13,10 +13,6 @@ The I2C address is 0X69. ## PSM node This ros node just publishes the current and voltage data we get from the lib to the entire ROS environment. -### Publishes to +### Publishes to * /asv/power_sense_module/current for the current data [A] * /asv/power_sense_module/voltage for the voltage data [V] - - - - 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..b7cf34f74 100755 --- a/mission/internal_status_auv/internal_status_auv/power_sense_module_lib.py +++ b/mission/internal_status_auv/internal_status_auv/power_sense_module_lib.py @@ -1,11 +1,12 @@ -# Libraies for Power Sense Module +# Libraries for Power Sense Module import time + import smbus from MCP342x import MCP342x 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 @@ -16,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 @@ -31,12 +32,23 @@ 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 - def get_voltage(self): + def get_voltage(self) -> float: + """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 = ( @@ -47,7 +59,18 @@ 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). + + 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..851f9da4f 100755 --- a/mission/internal_status_auv/internal_status_auv/power_sense_module_node.py +++ b/mission/internal_status_auv/internal_status_auv/power_sense_module_node.py @@ -1,19 +1,19 @@ #!/usr/bin/env python3 # ROS2 Libraries +# Custom Libraries 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): + def __init__(self) -> None: # 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( @@ -42,14 +42,14 @@ def __init__(self): self.logger = get_logger("power_sense_module") self.declare_parameter("internal_status.voltage_min", 14.5) - self.voltageMin = ( + 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.voltage_max = ( self.get_parameter("internal_status.voltage_max") .get_parameter_value() .double_value @@ -66,13 +66,18 @@ def __init__(self): 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): + def read_timer_callback(self) -> None: + """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() + self.current = self.psm.get_current() + self.voltage = self.psm.get_voltage() # Publish PSM data current_msg = Float32() @@ -88,15 +93,30 @@ def read_timer_callback(self): voltage_msg ) # publish voltage value to the "voltge topic" - def warning_timer_callback(self): - # Check if Voltage is abnormal and if so print a warning - if self.voltage < self.voltageMin: + def warning_timer_callback(self) -> None: + """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.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") -def main(args=None): +def main(args: list = None) -> 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..763bf8ad7 100755 --- a/mission/internal_status_auv/internal_status_auv/pressure_sensor_lib.py +++ b/mission/internal_status_auv/internal_status_auv/pressure_sensor_lib.py @@ -2,22 +2,23 @@ # 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: - def __init__(self): + def __init__(self) -> None: # 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, @@ -28,7 +29,18 @@ def __init__(self): time.sleep(1) - def get_pressure(self): + def get_pressure(self) -> float: + """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..fe55ec3be 100755 --- a/mission/internal_status_auv/internal_status_auv/pressure_sensor_node.py +++ b/mission/internal_status_auv/internal_status_auv/pressure_sensor_node.py @@ -1,18 +1,18 @@ #!/usr/bin/env python3 # ROS2 Libraries +# Custom Libraries 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): + def __init__(self) -> None: # 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") @@ -38,7 +38,7 @@ def __init__(self): self.logger = get_logger("pressure_sensor") self.declare_parameter("internal_status.pressure_critical_level", 1000.0) - self.pressureCriticalLevel = ( + self.pressure_critical_level = ( self.get_parameter("internal_status.pressure_critical_level") .get_parameter_value() .double_value @@ -55,27 +55,47 @@ def __init__(self): warning_timer_period, self.warning_timer_callback ) - # Debuging ---------- + # 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. + + 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() + self.pressure = self.pressure.get_pressure() # Publish pressure data pressure_msg = Float32() pressure_msg.data = self.pressure self.publisher_pressure.publish(pressure_msg) - def warning_timer_callback(self): - # Check if Pressure is abnormaly to high, if so print a warning - if self.pressure > self.pressureCriticalLevel: + def warning_timer_callback(self) -> None: + """Callback function triggered by the warning timer. + + This function checks if the pressure exceeds the critical level. + If so, a fatal warning is logged indicating a possible leak in the AUV. + """ + if self.pressure > self.pressure_critical_level: self.logger.fatal( f"WARNING: Internal pressure to HIGH: {self.pressure} hPa! Drone might be LEAKING!" ) -def main(args=None): +def main(args: list = None) -> 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 807a672d1..c0bada58b 100755 --- a/mission/internal_status_auv/internal_status_auv/temperature_sensor_lib.py +++ b/mission/internal_status_auv/internal_status_auv/temperature_sensor_lib.py @@ -1,9 +1,9 @@ #!/usr/bin/python3 -""" -! NOTE: -! 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 +"""! NOTE. + +! For now we don't have a external sensor to measure internal temperature +! Instead we just use Internal Computer temperature sensor to gaugue temperature of the environment approximately +! In the future someone should implement a external temperature sensor for measuting a more accurate state of the temperatuer on the inside of the AUV. """ # Python Libraries @@ -11,17 +11,28 @@ 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. + + 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( ["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_node.py b/mission/internal_status_auv/internal_status_auv/temperature_sensor_node.py index 43f93f096..49f6d5299 100755 --- a/mission/internal_status_auv/internal_status_auv/temperature_sensor_node.py +++ b/mission/internal_status_auv/internal_status_auv/temperature_sensor_node.py @@ -1,18 +1,18 @@ #!/usr/bin/python3 # ROS2 Libraries +# Custom Libraries 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): + def __init__(self) -> None: # Pressure sensor setup ---------- - self.Temperature = ( + self.temperature = ( internal_status_auv.temperature_sensor_lib.TemperatureSensor() ) @@ -42,7 +42,7 @@ def __init__(self): self.logger = get_logger("temperature_sensor") self.declare_parameter("internal_status.temperature_critical_level", 90.0) - self.temperatureCriticalLevel = ( + self.temperature_critical_level = ( self.get_parameter("internal_status.temperature_critical_level") .get_parameter_value() .double_value @@ -59,27 +59,47 @@ def __init__(self): warning_timer_period, self.warning_timer_callback ) - # Debuging ---------- + # 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. + + 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() + self.temperature = self.temperature.get_temperature() # Publish temperature data temperature_msg = Float32() temperature_msg.data = self.temperature self.publisher_temperature.publish(temperature_msg) - def warning_timer_callback(self): - # Check if Temperature is abnormal and if so print a warning - if self.temperature > self.temperatureCriticalLevel: + def warning_timer_callback(self) -> None: + """Callback function triggered by the warning timer. + + This function checks if the temperature exceeds the critical level. + If so, a fatal warning is logged indicating a possible overheating situation. + """ + if self.temperature > self.temperature_critical_level: self.logger.fatal( f"WARNING: Temperature inside the Drone to HIGH: {self.temperature} *C! Drone might be overheating!" ) -def main(args=None): +def main(args: list = None) -> 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..399018e33 100644 --- a/mission/internal_status_auv/launch/internal_status_auv.launch.py +++ b/mission/internal_status_auv/launch/internal_status_auv.launch.py @@ -1,10 +1,26 @@ +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(): +def generate_launch_description() -> LaunchDescription: + """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/internal_status_auv/startup_scripts/internal_status_auv.service b/mission/internal_status_auv/startup_scripts/internal_status_auv.service index 240030ff0..d2191ac23 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,8 +13,8 @@ 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] -WantedBy=multi-user.target \ No newline at end of file +WantedBy=multi-user.target diff --git a/mission/internal_status_auv/startup_scripts/internal_status_auv.sh b/mission/internal_status_auv/startup_scripts/internal_status_auv.sh index 8a26eaf98..cd63cdf2d 100644 --- a/mission/internal_status_auv/startup_scripts/internal_status_auv.sh +++ b/mission/internal_status_auv/startup_scripts/internal_status_auv.sh @@ -2,6 +2,6 @@ SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" cd $SCRIPT_DIR -cd ../../../../.. # Go back to workspace +cd ../../../../.. # Go back to workspace source install/setup.bash ros2 launch internal_status_auv internal_status_auv.launch.py diff --git a/mission/joystick_interface_auv/README.md b/mission/joystick_interface_auv/README.md index 2c54e8e49..36ae93c7b 100644 --- a/mission/joystick_interface_auv/README.md +++ b/mission/joystick_interface_auv/README.md @@ -1,3 +1,2 @@ ## Joystick interface A joystick interface for manual control of AUV. A ROS2 node that subscribes on inputs from the XBOX controller and publishes the according wrench message to be used in Thruster Allocation. - 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..4bf665328 100755 --- a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv.py +++ b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv.py @@ -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: @@ -74,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." @@ -135,8 +134,7 @@ def create_wrench_message( pitch: float, yaw: float, ) -> Wrench: - """ - Creates a 2D wrench message with the given x, y, heave, roll, pitch, and yaw values. + """Creates a 2D wrench message with the given x, y, heave, roll, pitch, and yaw values. Args: surge (float): The x component of the force vector. @@ -158,28 +156,22 @@ def create_wrench_message( wrench_msg.torque.z = yaw return wrench_msg - def transition_to_xbox_mode(self): - """ - Turns off the controller and signals that the operational mode has switched to Xbox mode. - """ + 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): - """ - Publishes a zero force wrench message and signals that the system is turning on autonomous mode. - """ + def transition_to_autonomous_mode(self) -> None: + """Publishes a zero force wrench message and signals that the system is turning on autonomous mode.""" wrench_msg = self.create_wrench_message(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) self.wrench_publisher_.publish(wrench_msg) self.operational_mode_signal_publisher_.publish(String(data="autonomous mode")) self.state_ = States.AUTONOMOUS_MODE def joystick_cb(self, msg: Joy) -> Wrench: - """ - Callback function that receives joy messages and converts them into - wrench messages to be sent to the thruster allocation node. - Handles software killswitch and control mode buttons, - and transitions between different states of operation. + """Callback function that receives joy messages and converts them into wrench messages to be sent to the thruster allocation node. + + Handles software killswitch and control mode buttons, and transitions between different states of operation. Args: msg: A ROS message containing the joy input data. @@ -187,8 +179,7 @@ def joystick_cb(self, msg: Joy) -> Wrench: Returns: A ROS message containing the wrench data that was sent to the thruster allocation node. """ - - current_time = self.get_clock().now().to_msg()._sec + current_time = self.get_clock().now().to_msg().sec buttons = {} axes = {} @@ -283,16 +274,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)) + 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 + # 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: @@ -322,7 +312,13 @@ 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 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..f99e3b32d 100755 --- a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py +++ b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py @@ -1,10 +1,21 @@ 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(): +def generate_launch_description() -> LaunchDescription: + """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/package.xml b/mission/joystick_interface_auv/package.xml index 8bfb67457..6583ce0ea 100644 --- a/mission/joystick_interface_auv/package.xml +++ b/mission/joystick_interface_auv/package.xml @@ -4,13 +4,13 @@ joystick_interface_auv 0.0.0 Joystick interface package - + aleksos - + MIT - + ament_cmake_python - + rclpy geometry_msgs sensor_msgs 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..bf4c38e47 100644 --- a/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py +++ b/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py @@ -1,13 +1,26 @@ -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 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. + + 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 @@ -19,7 +32,27 @@ 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. + + 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] @@ -34,7 +67,25 @@ 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. + + 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 @@ -51,7 +102,25 @@ 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. + + 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/CMakeLists.txt b/motion/thrust_allocator_auv/CMakeLists.txt index 21d473ffa..ae9714112 100644 --- a/motion/thrust_allocator_auv/CMakeLists.txt +++ b/motion/thrust_allocator_auv/CMakeLists.txt @@ -19,15 +19,15 @@ find_package(Eigen3 REQUIRED) include_directories(include) -add_executable(${PROJECT_NAME}_node - src/thrust_allocator_auv_node.cpp - src/thrust_allocator_ros.cpp +add_executable(${PROJECT_NAME}_node + src/thrust_allocator_auv_node.cpp + src/thrust_allocator_ros.cpp src/pseudoinverse_allocator.cpp ) -ament_target_dependencies(${PROJECT_NAME}_node - rclcpp - geometry_msgs +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + geometry_msgs vortex_msgs Eigen3 ) diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/eigen_vector6d_typedef.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/eigen_vector6d_typedef.hpp index f7c7dcfde..e06511362 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/eigen_vector6d_typedef.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/eigen_vector6d_typedef.hpp @@ -12,4 +12,4 @@ namespace Eigen { typedef Eigen::Matrix Vector6d; } -#endif // VORTEX_EIGEN_TYPEDEFS_H \ No newline at end of file +#endif // VORTEX_EIGEN_TYPEDEFS_H diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp index afe775cef..e1d96ce9f 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp @@ -15,25 +15,25 @@ * the input torques using the pseudoinverse allocator. */ class PseudoinverseAllocator { -public: - /** - * @brief Constructor for the PseudoinverseAllocator class. - * - * @param T_pinv The pseudoinverse of the thruster configuration matrix. - */ - explicit PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv); + public: + /** + * @brief Constructor for the PseudoinverseAllocator class. + * + * @param T_pinv The pseudoinverse of the thruster configuration matrix. + */ + explicit PseudoinverseAllocator(const Eigen::MatrixXd& T_pinv); - /** - * @brief Calculates the allocated thrust given the input torques using the - * pseudoinverse allocator. - * - * @param tau The input torques as a vector. - * @return The allocated thrust as a vector. - */ - Eigen::VectorXd calculate_allocated_thrust(const Eigen::VectorXd &tau); + /** + * @brief Calculates the allocated thrust given the input torques using the + * pseudoinverse allocator. + * + * @param tau The input torques as a vector. + * @return The allocated thrust as a vector. + */ + Eigen::VectorXd calculate_allocated_thrust(const Eigen::VectorXd& tau); - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - Eigen::MatrixXd T_pinv; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::MatrixXd T_pinv; }; -#endif // VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP \ No newline at end of file +#endif // VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp index 790d4b50f..0ef1a37f3 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp @@ -6,63 +6,63 @@ #ifndef VORTEX_ALLOCATOR_ROS_HPP #define VORTEX_ALLOCATOR_ROS_HPP -#include "thrust_allocator_auv/eigen_vector6d_typedef.hpp" -#include "thrust_allocator_auv/pseudoinverse_allocator.hpp" -#include "thrust_allocator_auv/thrust_allocator_utils.hpp" #include #include #include #include +#include "thrust_allocator_auv/eigen_vector6d_typedef.hpp" +#include "thrust_allocator_auv/pseudoinverse_allocator.hpp" +#include "thrust_allocator_auv/thrust_allocator_utils.hpp" class ThrustAllocator : public rclcpp::Node { -public: - explicit ThrustAllocator(); + public: + explicit ThrustAllocator(); - /** - * @brief Calculates the allocated - * thrust based on the body frame forces. It then saturates the output vector - * between min and max values and publishes the thruster forces to the topic - * "thrust/thruster_forces". - */ - void calculate_thrust_timer_cb(); + /** + * @brief Calculates the allocated + * thrust based on the body frame forces. It then saturates the output + * vector between min and max values and publishes the thruster forces to + * the topic "thrust/thruster_forces". + */ + void calculate_thrust_timer_cb(); -private: - /** - * @brief Callback function for the wrench input subscription. Extracts the - * surge, sway, heave, roll, pitch and yaw values from the received wrench msg - * and stores them in the body_frame_forces_ Eigen vector. - * @param msg The received geometry_msgs::msg::Wrench message. - */ - void wrench_cb(const geometry_msgs::msg::Wrench &msg); + private: + /** + * @brief Callback function for the wrench input subscription. Extracts the + * surge, sway, heave, roll, pitch and yaw values from the received wrench + * msg and stores them in the body_frame_forces_ Eigen vector. + * @param msg The received geometry_msgs::msg::Wrench message. + */ + void wrench_cb(const geometry_msgs::msg::Wrench& msg); - /** - * @brief Checks if the given Eigen vector contains any NaN or Inf values - * @param v The Eigen vector to check. - * @return True if the vector is healthy, false otherwise. - */ - bool healthy_wrench(const Eigen::VectorXd &v) const; + /** + * @brief Checks if the given Eigen vector contains any NaN or Inf values + * @param v The Eigen vector to check. + * @return True if the vector is healthy, false otherwise. + */ + bool healthy_wrench(const Eigen::VectorXd& v) const; - rclcpp::Publisher::SharedPtr - thruster_forces_publisher_; - rclcpp::Subscription::SharedPtr - wrench_subscriber_; - rclcpp::TimerBase::SharedPtr calculate_thrust_timer_; + rclcpp::Publisher::SharedPtr + thruster_forces_publisher_; + rclcpp::Subscription::SharedPtr + wrench_subscriber_; + rclcpp::TimerBase::SharedPtr calculate_thrust_timer_; - size_t count_; - Eigen::Vector3d center_of_mass_; - int num_dimensions_; - int num_thrusters_; - int min_thrust_; - int max_thrust_; + size_t count_; + Eigen::Vector3d center_of_mass_; + int num_dimensions_; + int num_thrusters_; + int min_thrust_; + int max_thrust_; - std::chrono::milliseconds thrust_update_period_; + std::chrono::milliseconds thrust_update_period_; - Eigen::MatrixXd thruster_force_direction_; - Eigen::MatrixXd thruster_position_; - Eigen::MatrixXd thrust_configuration_; + Eigen::MatrixXd thruster_force_direction_; + Eigen::MatrixXd thruster_position_; + Eigen::MatrixXd thrust_configuration_; - Eigen::Vector6d body_frame_forces_; - PseudoinverseAllocator pseudoinverse_allocator_; + Eigen::Vector6d body_frame_forces_; + PseudoinverseAllocator pseudoinverse_allocator_; }; -#endif // VORTEX_ALLOCATOR_ROS_HPP +#endif // VORTEX_ALLOCATOR_ROS_HPP diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp index bf6848bcf..9ca8aac07 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp @@ -22,36 +22,36 @@ * @return true if the matrix has any NaN or INF elements, false otherwise. */ template -inline bool is_invalid_matrix(const Eigen::MatrixBase &M) { - bool has_nan = !(M.array() == M.array()).all(); - bool has_inf = M.array().isInf().any(); - return has_nan || has_inf; +inline bool is_invalid_matrix(const Eigen::MatrixBase& M) { + bool has_nan = !(M.array() == M.array()).all(); + bool has_inf = M.array().isInf().any(); + return has_nan || has_inf; } inline Eigen::MatrixXd calculate_thrust_allocation_matrix( - const Eigen::MatrixXd &thruster_force_direction, - const Eigen::MatrixXd &thruster_position, - const Eigen::Vector3d ¢er_of_mass) { - // Initialize thrust allocation matrix - Eigen::MatrixXd thrust_allocation_matrix = Eigen::MatrixXd::Zero(6, 8); - - // Calculate thrust and moment contributions from each thruster - for (int i = 0; i < thruster_position.cols(); i++) { - Eigen::Vector3d pos = - thruster_position.col(i); // Thrust vector in body frame - Eigen::Vector3d F = - thruster_force_direction.col(i); // Position vector in body frame - - // Calculate position vector relative to the center of mass - pos -= center_of_mass; - - // Fill in the thrust allocation matrix - thrust_allocation_matrix.block<3, 1>(0, i) = F; - thrust_allocation_matrix.block<3, 1>(3, i) = pos.cross(F); - } - - thrust_allocation_matrix = thrust_allocation_matrix.array(); - return thrust_allocation_matrix; + const Eigen::MatrixXd& thruster_force_direction, + const Eigen::MatrixXd& thruster_position, + const Eigen::Vector3d& center_of_mass) { + // Initialize thrust allocation matrix + Eigen::MatrixXd thrust_allocation_matrix = Eigen::MatrixXd::Zero(6, 8); + + // Calculate thrust and moment contributions from each thruster + for (int i = 0; i < thruster_position.cols(); i++) { + Eigen::Vector3d pos = + thruster_position.col(i); // Thrust vector in body frame + Eigen::Vector3d F = + thruster_force_direction.col(i); // Position vector in body frame + + // Calculate position vector relative to the center of mass + pos -= center_of_mass; + + // Fill in the thrust allocation matrix + thrust_allocation_matrix.block<3, 1>(0, i) = F; + thrust_allocation_matrix.block<3, 1>(3, i) = pos.cross(F); + } + + thrust_allocation_matrix = thrust_allocation_matrix.array(); + return thrust_allocation_matrix; } /** @@ -61,12 +61,13 @@ inline Eigen::MatrixXd calculate_thrust_allocation_matrix( * @throws char* if the pseudoinverse is invalid. * @return The pseudoinverse of the given matrix. */ -inline Eigen::MatrixXd calculate_right_pseudoinverse(const Eigen::MatrixXd &T) { - Eigen::MatrixXd pseudoinverse = T.transpose() * (T * T.transpose()).inverse(); - if (is_invalid_matrix(pseudoinverse)) { - throw std::runtime_error("Invalid Psuedoinverse Calculated"); - } - return pseudoinverse; +inline Eigen::MatrixXd calculate_right_pseudoinverse(const Eigen::MatrixXd& T) { + Eigen::MatrixXd pseudoinverse = + T.transpose() * (T * T.transpose()).inverse(); + if (is_invalid_matrix(pseudoinverse)) { + throw std::runtime_error("Invalid Pseudoinverse Calculated"); + } + return pseudoinverse; } /** @@ -79,17 +80,18 @@ inline Eigen::MatrixXd calculate_right_pseudoinverse(const Eigen::MatrixXd &T) { * @return True if all vector values are within the given range, false * otherwise. */ -inline bool saturate_vector_values(Eigen::VectorXd &vec, double min, +inline bool saturate_vector_values(Eigen::VectorXd& vec, + double min, double max) { - bool all_values_in_range = - std::all_of(vec.begin(), vec.end(), - [min, max](double val) { return val >= min && val <= max; }); + bool all_values_in_range = std::all_of( + vec.begin(), vec.end(), + [min, max](double val) { return val >= min && val <= max; }); - std::transform(vec.begin(), vec.end(), vec.begin(), [min, max](double val) { - return std::min(std::max(val, min), max); - }); + std::transform(vec.begin(), vec.end(), vec.begin(), [min, max](double val) { + return std::min(std::max(val, min), max); + }); - return all_values_in_range; + return all_values_in_range; } /** @@ -100,12 +102,12 @@ inline bool saturate_vector_values(Eigen::VectorXd &vec, double min, * @param msg The vortex_msgs::msg::ThrusterForces message to store the * converted values. */ -inline void array_eigen_to_msg(const Eigen::VectorXd &u, - vortex_msgs::msg::ThrusterForces &msg) { - int r = u.size(); - std::vector u_vec(r); - std::copy_n(u.begin(), r, u_vec.begin()); - msg.thrust = u_vec; +inline void array_eigen_to_msg(const Eigen::VectorXd& u, + vortex_msgs::msg::ThrusterForces& msg) { + int r = u.size(); + std::vector u_vec(r); + std::copy_n(u.begin(), r, u_vec.begin()); + msg.thrust = u_vec; } /** @@ -116,23 +118,25 @@ inline void array_eigen_to_msg(const Eigen::VectorXd &u, * @param cols The number of columns in the resulting Eigen matrix. * @return The resulting Eigen matrix. */ -inline Eigen::MatrixXd -double_array_to_eigen_matrix(const std::vector &matrix, int rows, - int cols) { - return Eigen::Map>(matrix.data(), rows, - cols); +inline Eigen::MatrixXd double_array_to_eigen_matrix( + const std::vector& matrix, + int rows, + int cols) { + return Eigen::Map>( + matrix.data(), rows, cols); } -inline Eigen::Vector3d -double_array_to_eigen_vector3d(const std::vector &vector) { - // Ensure the input vector has exactly three elements - if (vector.size() != 3) { - throw std::invalid_argument("Input vector must have exactly 3 elements"); - } +inline Eigen::Vector3d double_array_to_eigen_vector3d( + const std::vector& vector) { + // Ensure the input vector has exactly three elements + if (vector.size() != 3) { + throw std::invalid_argument( + "Input vector must have exactly 3 elements"); + } - // Map the vector to Eigen::Vector3d - return Eigen::Map(vector.data()); + // Map the vector to Eigen::Vector3d + return Eigen::Map(vector.data()); } -#endif // VORTEX_ALLOCATOR_UTILS_HPP \ No newline at end of file +#endif // VORTEX_ALLOCATOR_UTILS_HPP diff --git a/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py b/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py index 8d24d75bf..7a9c64b3c 100644 --- a/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py +++ b/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py @@ -1,10 +1,23 @@ from os import path + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node -def generate_launch_description(): +def generate_launch_description() -> LaunchDescription: + """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/thrust_allocator_auv/src/pseudoinverse_allocator.cpp b/motion/thrust_allocator_auv/src/pseudoinverse_allocator.cpp index 72b45a7b2..47ab046f0 100644 --- a/motion/thrust_allocator_auv/src/pseudoinverse_allocator.cpp +++ b/motion/thrust_allocator_auv/src/pseudoinverse_allocator.cpp @@ -1,10 +1,10 @@ #include "thrust_allocator_auv/pseudoinverse_allocator.hpp" -PseudoinverseAllocator::PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv) +PseudoinverseAllocator::PseudoinverseAllocator(const Eigen::MatrixXd& T_pinv) : T_pinv(T_pinv) {} -Eigen::VectorXd -PseudoinverseAllocator::calculate_allocated_thrust(const Eigen::VectorXd &tau) { - Eigen::VectorXd u = T_pinv * tau; - return u; -} \ No newline at end of file +Eigen::VectorXd PseudoinverseAllocator::calculate_allocated_thrust( + const Eigen::VectorXd& tau) { + Eigen::VectorXd u = T_pinv * tau; + return u; +} diff --git a/motion/thrust_allocator_auv/src/thrust_allocator_auv_node.cpp b/motion/thrust_allocator_auv/src/thrust_allocator_auv_node.cpp index fb6e3aac2..503a2ab36 100644 --- a/motion/thrust_allocator_auv/src/thrust_allocator_auv_node.cpp +++ b/motion/thrust_allocator_auv/src/thrust_allocator_auv_node.cpp @@ -1,11 +1,11 @@ #include "thrust_allocator_auv/thrust_allocator_ros.hpp" #include "thrust_allocator_auv/thrust_allocator_utils.hpp" -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - auto allocator = std::make_shared(); - RCLCPP_INFO(allocator->get_logger(), "Thrust allocator initiated"); - rclcpp::spin(allocator); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto allocator = std::make_shared(); + RCLCPP_INFO(allocator->get_logger(), "Thrust allocator initiated"); + rclcpp::spin(allocator); + rclcpp::shutdown(); + return 0; +} diff --git a/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp b/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp index 0f6ee771a..af835f635 100644 --- a/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp +++ b/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp @@ -1,7 +1,7 @@ #include "thrust_allocator_auv/thrust_allocator_ros.hpp" +#include #include "thrust_allocator_auv/pseudoinverse_allocator.hpp" #include "thrust_allocator_auv/thrust_allocator_utils.hpp" -#include #include #include @@ -11,99 +11,101 @@ using namespace std::chrono_literals; ThrustAllocator::ThrustAllocator() : Node("thrust_allocator_node"), pseudoinverse_allocator_(Eigen::MatrixXd::Zero(6, 8)) { - declare_parameter("physical.center_of_mass", std::vector{0}); - declare_parameter("propulsion.dimensions.num", 3); - declare_parameter("propulsion.thrusters.num", 8); - declare_parameter("propulsion.thrusters.min", -100); - declare_parameter("propulsion.thrusters.max", 100); - declare_parameter("propulsion.thrusters.thrust_update_rate", 10.0); - declare_parameter("propulsion.thrusters.thruster_force_direction", - std::vector{0}); - declare_parameter("propulsion.thrusters.thruster_position", - std::vector{0}); - - center_of_mass_ = double_array_to_eigen_vector3d( - get_parameter("physical.center_of_mass").as_double_array()); - - num_dimensions_ = get_parameter("propulsion.dimensions.num").as_int(); - num_thrusters_ = get_parameter("propulsion.thrusters.num").as_int(); - min_thrust_ = get_parameter("propulsion.thrusters.min").as_int(); - max_thrust_ = get_parameter("propulsion.thrusters.max").as_int(); - thrust_update_period_ = std::chrono::milliseconds(static_cast( - 1000 / - get_parameter("propulsion.thrusters.thrust_update_rate").as_double())); - - thruster_force_direction_ = double_array_to_eigen_matrix( - get_parameter("propulsion.thrusters.thruster_force_direction") - .as_double_array(), - num_dimensions_, num_thrusters_); - - thruster_position_ = double_array_to_eigen_matrix( - get_parameter("propulsion.thrusters.thruster_position").as_double_array(), - num_dimensions_, num_thrusters_); - - thrust_configuration_ = calculate_thrust_allocation_matrix( - thruster_force_direction_, thruster_position_, center_of_mass_); - - wrench_subscriber_ = this->create_subscription( - "thrust/wrench_input", 1, - std::bind(&ThrustAllocator::wrench_cb, this, std::placeholders::_1)); - - thruster_forces_publisher_ = - this->create_publisher( - "thrust/thruster_forces", 5); - - calculate_thrust_timer_ = this->create_wall_timer( - thrust_update_period_, - std::bind(&ThrustAllocator::calculate_thrust_timer_cb, this)); - - pseudoinverse_allocator_.T_pinv = - calculate_right_pseudoinverse(thrust_configuration_); - - body_frame_forces_.setZero(); + declare_parameter("physical.center_of_mass", std::vector{0}); + declare_parameter("propulsion.dimensions.num", 3); + declare_parameter("propulsion.thrusters.num", 8); + declare_parameter("propulsion.thrusters.min", -100); + declare_parameter("propulsion.thrusters.max", 100); + declare_parameter("propulsion.thrusters.thrust_update_rate", 10.0); + declare_parameter("propulsion.thrusters.thruster_force_direction", + std::vector{0}); + declare_parameter("propulsion.thrusters.thruster_position", + std::vector{0}); + + center_of_mass_ = double_array_to_eigen_vector3d( + get_parameter("physical.center_of_mass").as_double_array()); + + num_dimensions_ = get_parameter("propulsion.dimensions.num").as_int(); + num_thrusters_ = get_parameter("propulsion.thrusters.num").as_int(); + min_thrust_ = get_parameter("propulsion.thrusters.min").as_int(); + max_thrust_ = get_parameter("propulsion.thrusters.max").as_int(); + thrust_update_period_ = std::chrono::milliseconds(static_cast( + 1000 / + get_parameter("propulsion.thrusters.thrust_update_rate").as_double())); + + thruster_force_direction_ = double_array_to_eigen_matrix( + get_parameter("propulsion.thrusters.thruster_force_direction") + .as_double_array(), + num_dimensions_, num_thrusters_); + + thruster_position_ = double_array_to_eigen_matrix( + get_parameter("propulsion.thrusters.thruster_position") + .as_double_array(), + num_dimensions_, num_thrusters_); + + thrust_configuration_ = calculate_thrust_allocation_matrix( + thruster_force_direction_, thruster_position_, center_of_mass_); + + wrench_subscriber_ = this->create_subscription( + "thrust/wrench_input", 1, + std::bind(&ThrustAllocator::wrench_cb, this, std::placeholders::_1)); + + thruster_forces_publisher_ = + this->create_publisher( + "thrust/thruster_forces", 5); + + calculate_thrust_timer_ = this->create_wall_timer( + thrust_update_period_, + std::bind(&ThrustAllocator::calculate_thrust_timer_cb, this)); + + pseudoinverse_allocator_.T_pinv = + calculate_right_pseudoinverse(thrust_configuration_); + + body_frame_forces_.setZero(); } void ThrustAllocator::calculate_thrust_timer_cb() { - Eigen::VectorXd thruster_forces = - pseudoinverse_allocator_.calculate_allocated_thrust(body_frame_forces_); - - if (is_invalid_matrix(thruster_forces)) { - RCLCPP_ERROR(get_logger(), "ThrusterForces vector invalid"); - return; - } - - if (!saturate_vector_values(thruster_forces, min_thrust_, max_thrust_)) { - RCLCPP_WARN(get_logger(), "Thruster forces vector required saturation."); - } - - vortex_msgs::msg::ThrusterForces msg_out; - array_eigen_to_msg(thruster_forces, msg_out); - thruster_forces_publisher_->publish(msg_out); + Eigen::VectorXd thruster_forces = + pseudoinverse_allocator_.calculate_allocated_thrust(body_frame_forces_); + + if (is_invalid_matrix(thruster_forces)) { + RCLCPP_ERROR(get_logger(), "ThrusterForces vector invalid"); + return; + } + + if (!saturate_vector_values(thruster_forces, min_thrust_, max_thrust_)) { + RCLCPP_WARN(get_logger(), + "Thruster forces vector required saturation."); + } + + vortex_msgs::msg::ThrusterForces msg_out; + array_eigen_to_msg(thruster_forces, msg_out); + thruster_forces_publisher_->publish(msg_out); } -void ThrustAllocator::wrench_cb(const geometry_msgs::msg::Wrench &msg) { - Eigen::Vector6d msg_vector; - msg_vector(0) = msg.force.x; // surge - msg_vector(1) = msg.force.y; // sway - msg_vector(2) = msg.force.z; // heave - msg_vector(3) = msg.torque.x; // roll - msg_vector(4) = msg.torque.y; // pitch - msg_vector(5) = msg.torque.z; // yaw - - if (!healthy_wrench(msg_vector)) { - RCLCPP_ERROR(get_logger(), "ASV wrench vector invalid, ignoring."); - return; - } - std::swap(msg_vector, body_frame_forces_); +void ThrustAllocator::wrench_cb(const geometry_msgs::msg::Wrench& msg) { + Eigen::Vector6d msg_vector; + msg_vector(0) = msg.force.x; // surge + msg_vector(1) = msg.force.y; // sway + msg_vector(2) = msg.force.z; // heave + msg_vector(3) = msg.torque.x; // roll + msg_vector(4) = msg.torque.y; // pitch + msg_vector(5) = msg.torque.z; // yaw + + if (!healthy_wrench(msg_vector)) { + RCLCPP_ERROR(get_logger(), "ASV wrench vector invalid, ignoring."); + return; + } + std::swap(msg_vector, body_frame_forces_); } -bool ThrustAllocator::healthy_wrench(const Eigen::VectorXd &v) const { - if (is_invalid_matrix(v)) - return false; +bool ThrustAllocator::healthy_wrench(const Eigen::VectorXd& v) const { + if (is_invalid_matrix(v)) + return false; - bool within_max_thrust = std::none_of(v.begin(), v.end(), [this](double val) { - return std::abs(val) > max_thrust_; - }); + bool within_max_thrust = std::none_of( + v.begin(), v.end(), + [this](double val) { return std::abs(val) > max_thrust_; }); - return within_max_thrust; + return within_max_thrust; } diff --git a/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py b/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py index a16df2483..944164e5c 100644 --- a/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py +++ b/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py @@ -1,10 +1,22 @@ +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(): +def generate_launch_description() -> LaunchDescription: + """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/package.xml b/motion/thruster_interface_auv/package.xml index bce547c07..d0dc93e67 100644 --- a/motion/thruster_interface_auv/package.xml +++ b/motion/thruster_interface_auv/package.xml @@ -3,12 +3,12 @@ thruster_interface_auv 1.0.0 - Thruster interface to controll thrusters through PCA9685 Module + Thruster interface to control thrusters through PCA9685 Module vortex MIT ament_cmake_python - + rclpy std_msgs vortex_msgs diff --git a/motion/thruster_interface_auv/resources/README b/motion/thruster_interface_auv/resources/README index b5e226d38..acf71934f 100644 --- a/motion/thruster_interface_auv/resources/README +++ b/motion/thruster_interface_auv/resources/README @@ -1 +1 @@ -The different .CSV files reflect the different behaviours of the motors according to the voltage in the system \ No newline at end of file +The different .CSV files reflect the different behaviours of the motors according to the voltage in the system diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-10V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-10V.csv index 6689d4df1..de7294093 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-10V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-10V.csv @@ -1,202 +1,202 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) -1100,2662,13.62,10,136.2,-2.31,17.0 -1104,2667,13.63,10,136.3,-2.30,16.9 -1108,2661,13.45,10,134.5,-2.30,17.1 -1112,2627,13.2,10,132,-2.28,17.3 -1116,2617,12.81,10,128.1,-2.22,17.4 -1120,2600,12.5,10,125,-2.18,17.5 -1124,2590,12.14,10,121.4,-2.16,17.8 -1128,2562,11.96,10,119.6,-2.13,17.8 -1132,2535,11.63,10,116.3,-2.11,18.1 -1136,2528,11.32,10,113.2,-2.05,18.2 -1140,2514,11,10,110,-2.04,18.6 -1144,2484,10.8,10,108,-1.99,18.4 -1148,2472,10.45,10,104.5,-1.95,18.7 -1152,2449,10.2,10,102,-1.93,18.9 -1156,2436,9.9,10,99,-1.89,19.1 -1160,2416,9.56,10,95.6,-1.84,19.2 -1164,2395,9.29,10,92.9,-1.81,19.5 -1168,2371,9.1,10,91,-1.79,19.7 -1172,2344,8.8,10,88,-1.76,19.9 -1176,2332,8.5,10,85,-1.71,20.2 -1180,2306,8.3,10,83,-1.68,20.3 -1184,2283,8.05,10,80.5,-1.65,20.5 -1188,2271,7.71,10,77.1,-1.62,21.0 -1192,2241,7.57,10,75.7,-1.59,21.0 -1196,2219,7.3,10,73,-1.56,21.3 -1200,2198,7.1,10,71,-1.54,21.7 -1204,2177,6.82,10,68.2,-1.50,22.0 -1208,2157,6.6,10,66,-1.47,22.3 -1212,2129,6.48,10,64.8,-1.44,22.3 -1216,2106,6.2,10,62,-1.41,22.8 -1220,2094,6,10,60,-1.36,22.7 -1224,2073,5.8,10,58,-1.33,22.9 -1228,2046,5.6,10,56,-1.32,23.6 -1232,2024,5.4,10,54,-1.29,23.9 -1236,2005,5.2,10,52,-1.26,24.2 -1240,1982,5,10,50,-1.22,24.4 -1244,1957,4.84,10,48.4,-1.19,24.6 -1248,1936,4.64,10,46.4,-1.17,25.1 -1252,1907,4.5,10,45,-1.14,25.3 -1256,1885,4.3,10,43,-1.10,25.5 -1260,1864,4.1,10,41,-1.08,26.2 -1264,1834,3.9,10,39,-1.06,27.1 -1268,1816,3.73,10,37.3,-1.02,27.2 -1272,1789,3.6,10,36,-0.99,27.5 -1276,1762,3.5,10,35,-0.97,27.7 -1280,1745,3.3,10,33,-0.93,28.2 -1284,1717,3.2,10,32,-0.91,28.3 -1288,1697,3,10,30,-0.88,29.3 -1292,1671,2.9,10,29,-0.85,29.4 -1296,1643,2.8,10,28,-0.83,29.5 -1300,1615,2.6,10,26,-0.80,30.7 -1304,1591,2.4,10,24,-0.78,32.5 -1308,1562,2.3,10,23,-0.75,32.7 -1312,1539,2.1,10,21,-0.72,34.3 -1316,1516,2,10,20,-0.70,34.9 -1320,1488,1.9,10,19,-0.67,35.3 -1324,1461,1.8,10,18,-0.65,36.3 -1328,1440,1.7,10,17,-0.63,37.1 -1332,1411,1.6,10,16,-0.61,38.3 -1336,1382,1.5,10,15,-0.58,38.7 -1340,1347,1.4,10,14,-0.55,39.5 -1344,1320,1.3,10,13,-0.53,40.8 -1348,1294,1.2,10,12,-0.51,42.7 -1352,1265,1.2,10,12,-0.49,40.4 -1356,1239,1.1,10,11,-0.47,42.5 -1360,1200,1,10,10,-0.44,44.5 -1364,1174,0.9,10,9,-0.42,46.9 -1368,1142,0.8,10,8,-0.40,49.9 -1372,1114,0.8,10,8,-0.38,47.6 -1376,1080,0.7,10,7,-0.35,50.5 -1380,1048,0.7,10,7,-0.34,48.0 -1384,1016,0.6,10,6,-0.31,52.2 -1388,986,0.5,10,5,-0.29,59.0 -1392,948,0.5,10,5,-0.28,55.3 -1396,916,0.4,10,4,-0.25,62.4 -1400,883,0.4,10,4,-0.24,59.0 -1404,845,0.4,10,4,-0.21,53.3 -1408,813,0.3,10,3,-0.20,66.5 -1412,774,0.22,10,2.2,-0.18,82.5 -1416,740,0.2,10,2,-0.16,81.6 -1420,701,0.2,10,2,-0.15,77.1 -1424,664,0.2,10,2,-0.14,68.0 -1428,624,0.2,10,2,-0.12,59.0 -1432,584,0.1,10,1,-0.10,99.8 -1436,545,0.1,10,1,-0.09,86.2 -1440,504,0.1,10,1,-0.07,72.6 -1444,462,0.05,10,0.5,-0.06,127.0 -1448,421,0.05,10,0.5,-0.05,108.9 -1452,377,0.05,10,0.5,-0.05,90.7 -1456,335,0.05,10,0.5,-0.03,63.5 -1460,0,0,10,0,0.00,0.0 -1464,0,0,10,0,0.00,0.0 -1468,0,0,10,0,0.00,0.0 -1472,0,0,10,0,0.00,0.0 -1476,0,0,10,0,0.00,0.0 -1480,0,0,10,0,0.00,0.0 -1484,0,0,10,0,0.00,0.0 -1488,0,0,10,0,0.00,0.0 -1492,0,0,10,0,0.00,0.0 -1496,0,0,10,0,0.00,0.0 -1500,0,0,10,0,0.00,0.0 -1504,0,0,10,0,0.00,0.0 -1508,0,0,10,0,0.00,0.0 -1512,0,0,10,0,0.00,0.0 -1516,0,0,10,0,0.00,0.0 -1520,0,0,10,0,0.00,0.0 -1524,0,0,10,0,0.00,0.0 -1528,0,0,10,0,0.00,0.0 -1532,0,0,10,0,0.00,0.0 -1536,0,0,10,0,0.00,0.0 -1540,0,0,10,0,0.00,0.0 -1544,332,0.05,10,0.5,0.04,81.6 -1548,373,0.05,10,0.5,0.05,108.9 -1552,417,0.05,10,0.5,0.07,136.1 -1556,459,0.1,10,1,0.08,77.1 -1560,500,0.1,10,1,0.09,90.7 -1564,540,0.1,10,1,0.11,108.9 -1568,582,0.1,10,1,0.13,127.0 -1572,621,0.2,10,2,0.15,72.6 -1576,657,0.2,10,2,0.16,81.6 -1580,695,0.2,10,2,0.18,90.7 -1584,733,0.2,10,2,0.20,102.1 -1588,766,0.3,10,3,0.22,74.1 -1592,805,0.3,10,3,0.24,81.6 -1596,836,0.4,10,4,0.26,65.8 -1600,873,0.4,10,4,0.29,72.6 -1604,906,0.47,10,4.7,0.31,66.6 -1608,938,0.5,10,5,0.34,67.1 -1612,974,0.5,10,5,0.36,72.6 -1616,1007,0.6,10,6,0.39,64.3 -1620,1035,0.7,10,7,0.41,59.0 -1624,1066,0.7,10,7,0.44,63.5 -1628,1101,0.8,10,8,0.47,58.4 -1632,1129,0.83,10,8.3,0.49,59.6 -1636,1159,0.94,10,9.4,0.52,55.5 -1640,1188,1,10,10,0.55,54.9 -1644,1219,1.1,10,11,0.58,52.8 -1648,1250,1.2,10,12,0.61,50.7 -1652,1278,1.3,10,13,0.64,49.2 -1656,1303,1.4,10,14,0.68,48.9 -1660,1330,1.5,10,15,0.70,46.9 -1664,1363,1.5,10,15,0.73,48.4 -1668,1392,1.6,10,16,0.76,47.6 -1672,1418,1.8,10,18,0.80,44.4 -1676,1445,1.9,10,19,0.83,43.4 -1680,1473,2,10,20,0.85,42.6 -1684,1496,2.1,10,21,0.89,42.3 -1688,1519,2.2,10,22,0.92,41.9 -1692,1542,2.3,10,23,0.95,41.2 -1696,1571,2.43,10,24.3,0.98,40.1 -1700,1596,2.7,10,27,0.97,36.0 -1704,1622,2.8,10,28,1.05,37.4 -1708,1650,2.9,10,29,1.08,37.1 -1712,1674,3.1,10,31,1.11,35.8 -1716,1696,3.3,10,33,1.17,35.3 -1720,1720,3.4,10,34,1.18,34.8 -1724,1747,3.5,10,35,1.21,34.5 -1728,1765,3.7,10,37,1.24,33.5 -1732,1793,3.84,10,38.4,1.27,33.2 -1736,1819,4,10,40,1.31,32.8 -1740,1833,4.2,10,42,1.36,32.3 -1744,1860,4.4,10,44,1.41,32.1 -1748,1889,4.5,10,45,1.43,31.8 -1752,1911,4.7,10,47,1.48,31.5 -1756,1933,4.9,10,49,1.52,31.1 -1760,1962,5.1,10,51,1.55,30.3 -1764,1984,5.3,10,53,1.58,29.8 -1768,2004,5.5,10,55,1.62,29.5 -1772,2027,5.7,10,57,1.66,29.0 -1776,2052,5.9,10,59,1.69,28.7 -1780,2075,6.09,10,60.9,1.73,28.4 -1784,2093,6.3,10,63,1.77,28.2 -1788,2118,6.5,10,65,1.80,27.7 -1792,2140,6.73,10,67.3,1.85,27.4 -1796,2160,6.9,10,69,1.88,27.2 -1800,2179,7.13,10,71.3,1.91,26.8 -1804,2200,7.4,10,74,1.99,26.9 -1808,2225,7.6,10,76,2.00,26.3 -1812,2245,7.9,10,79,2.06,26.1 -1816,2273,8.1,10,81,2.09,25.8 -1820,2293,8.33,10,83.3,2.12,25.5 -1824,2307,8.64,10,86.4,2.17,25.1 -1828,2333,8.89,10,88.9,2.21,24.9 -1832,2353,9.14,10,91.4,2.27,24.9 -1836,2378,9.4,10,94,2.30,24.5 -1840,2398,9.7,10,97,2.36,24.4 -1844,2410,10,10,100,2.41,24.1 -1848,2436,10.2,10,102,2.45,24.1 -1852,2453,10.5,10,105,2.48,23.6 -1856,2481,10.79,10,107.9,2.55,23.7 -1860,2499,11.04,10,110.4,2.58,23.4 -1864,2522,11.35,10,113.5,2.62,23.1 -1868,2543,11.6,10,116,2.65,22.8 -1872,2558,11.9,10,119,2.69,22.6 -1876,2575,12.28,10,122.8,2.74,22.3 -1880,2602,12.44,10,124.4,2.77,22.3 -1884,2613,12.8,10,128,2.82,22.0 -1888,2633,13.13,10,131.3,2.88,21.9 -1892,2657,13.4,10,134,2.89,21.6 -1896,2665,13.62,10,136.2,2.92,21.4 -1900,2668,13.6,10,136,2.93,21.5 + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) +1100,2662,13.62,10,136.2,-2.31,17.0 +1104,2667,13.63,10,136.3,-2.30,16.9 +1108,2661,13.45,10,134.5,-2.30,17.1 +1112,2627,13.2,10,132,-2.28,17.3 +1116,2617,12.81,10,128.1,-2.22,17.4 +1120,2600,12.5,10,125,-2.18,17.5 +1124,2590,12.14,10,121.4,-2.16,17.8 +1128,2562,11.96,10,119.6,-2.13,17.8 +1132,2535,11.63,10,116.3,-2.11,18.1 +1136,2528,11.32,10,113.2,-2.05,18.2 +1140,2514,11,10,110,-2.04,18.6 +1144,2484,10.8,10,108,-1.99,18.4 +1148,2472,10.45,10,104.5,-1.95,18.7 +1152,2449,10.2,10,102,-1.93,18.9 +1156,2436,9.9,10,99,-1.89,19.1 +1160,2416,9.56,10,95.6,-1.84,19.2 +1164,2395,9.29,10,92.9,-1.81,19.5 +1168,2371,9.1,10,91,-1.79,19.7 +1172,2344,8.8,10,88,-1.76,19.9 +1176,2332,8.5,10,85,-1.71,20.2 +1180,2306,8.3,10,83,-1.68,20.3 +1184,2283,8.05,10,80.5,-1.65,20.5 +1188,2271,7.71,10,77.1,-1.62,21.0 +1192,2241,7.57,10,75.7,-1.59,21.0 +1196,2219,7.3,10,73,-1.56,21.3 +1200,2198,7.1,10,71,-1.54,21.7 +1204,2177,6.82,10,68.2,-1.50,22.0 +1208,2157,6.6,10,66,-1.47,22.3 +1212,2129,6.48,10,64.8,-1.44,22.3 +1216,2106,6.2,10,62,-1.41,22.8 +1220,2094,6,10,60,-1.36,22.7 +1224,2073,5.8,10,58,-1.33,22.9 +1228,2046,5.6,10,56,-1.32,23.6 +1232,2024,5.4,10,54,-1.29,23.9 +1236,2005,5.2,10,52,-1.26,24.2 +1240,1982,5,10,50,-1.22,24.4 +1244,1957,4.84,10,48.4,-1.19,24.6 +1248,1936,4.64,10,46.4,-1.17,25.1 +1252,1907,4.5,10,45,-1.14,25.3 +1256,1885,4.3,10,43,-1.10,25.5 +1260,1864,4.1,10,41,-1.08,26.2 +1264,1834,3.9,10,39,-1.06,27.1 +1268,1816,3.73,10,37.3,-1.02,27.2 +1272,1789,3.6,10,36,-0.99,27.5 +1276,1762,3.5,10,35,-0.97,27.7 +1280,1745,3.3,10,33,-0.93,28.2 +1284,1717,3.2,10,32,-0.91,28.3 +1288,1697,3,10,30,-0.88,29.3 +1292,1671,2.9,10,29,-0.85,29.4 +1296,1643,2.8,10,28,-0.83,29.5 +1300,1615,2.6,10,26,-0.80,30.7 +1304,1591,2.4,10,24,-0.78,32.5 +1308,1562,2.3,10,23,-0.75,32.7 +1312,1539,2.1,10,21,-0.72,34.3 +1316,1516,2,10,20,-0.70,34.9 +1320,1488,1.9,10,19,-0.67,35.3 +1324,1461,1.8,10,18,-0.65,36.3 +1328,1440,1.7,10,17,-0.63,37.1 +1332,1411,1.6,10,16,-0.61,38.3 +1336,1382,1.5,10,15,-0.58,38.7 +1340,1347,1.4,10,14,-0.55,39.5 +1344,1320,1.3,10,13,-0.53,40.8 +1348,1294,1.2,10,12,-0.51,42.7 +1352,1265,1.2,10,12,-0.49,40.4 +1356,1239,1.1,10,11,-0.47,42.5 +1360,1200,1,10,10,-0.44,44.5 +1364,1174,0.9,10,9,-0.42,46.9 +1368,1142,0.8,10,8,-0.40,49.9 +1372,1114,0.8,10,8,-0.38,47.6 +1376,1080,0.7,10,7,-0.35,50.5 +1380,1048,0.7,10,7,-0.34,48.0 +1384,1016,0.6,10,6,-0.31,52.2 +1388,986,0.5,10,5,-0.29,59.0 +1392,948,0.5,10,5,-0.28,55.3 +1396,916,0.4,10,4,-0.25,62.4 +1400,883,0.4,10,4,-0.24,59.0 +1404,845,0.4,10,4,-0.21,53.3 +1408,813,0.3,10,3,-0.20,66.5 +1412,774,0.22,10,2.2,-0.18,82.5 +1416,740,0.2,10,2,-0.16,81.6 +1420,701,0.2,10,2,-0.15,77.1 +1424,664,0.2,10,2,-0.14,68.0 +1428,624,0.2,10,2,-0.12,59.0 +1432,584,0.1,10,1,-0.10,99.8 +1436,545,0.1,10,1,-0.09,86.2 +1440,504,0.1,10,1,-0.07,72.6 +1444,462,0.05,10,0.5,-0.06,127.0 +1448,421,0.05,10,0.5,-0.05,108.9 +1452,377,0.05,10,0.5,-0.05,90.7 +1456,335,0.05,10,0.5,-0.03,63.5 +1460,0,0,10,0,0.00,0.0 +1464,0,0,10,0,0.00,0.0 +1468,0,0,10,0,0.00,0.0 +1472,0,0,10,0,0.00,0.0 +1476,0,0,10,0,0.00,0.0 +1480,0,0,10,0,0.00,0.0 +1484,0,0,10,0,0.00,0.0 +1488,0,0,10,0,0.00,0.0 +1492,0,0,10,0,0.00,0.0 +1496,0,0,10,0,0.00,0.0 +1500,0,0,10,0,0.00,0.0 +1504,0,0,10,0,0.00,0.0 +1508,0,0,10,0,0.00,0.0 +1512,0,0,10,0,0.00,0.0 +1516,0,0,10,0,0.00,0.0 +1520,0,0,10,0,0.00,0.0 +1524,0,0,10,0,0.00,0.0 +1528,0,0,10,0,0.00,0.0 +1532,0,0,10,0,0.00,0.0 +1536,0,0,10,0,0.00,0.0 +1540,0,0,10,0,0.00,0.0 +1544,332,0.05,10,0.5,0.04,81.6 +1548,373,0.05,10,0.5,0.05,108.9 +1552,417,0.05,10,0.5,0.07,136.1 +1556,459,0.1,10,1,0.08,77.1 +1560,500,0.1,10,1,0.09,90.7 +1564,540,0.1,10,1,0.11,108.9 +1568,582,0.1,10,1,0.13,127.0 +1572,621,0.2,10,2,0.15,72.6 +1576,657,0.2,10,2,0.16,81.6 +1580,695,0.2,10,2,0.18,90.7 +1584,733,0.2,10,2,0.20,102.1 +1588,766,0.3,10,3,0.22,74.1 +1592,805,0.3,10,3,0.24,81.6 +1596,836,0.4,10,4,0.26,65.8 +1600,873,0.4,10,4,0.29,72.6 +1604,906,0.47,10,4.7,0.31,66.6 +1608,938,0.5,10,5,0.34,67.1 +1612,974,0.5,10,5,0.36,72.6 +1616,1007,0.6,10,6,0.39,64.3 +1620,1035,0.7,10,7,0.41,59.0 +1624,1066,0.7,10,7,0.44,63.5 +1628,1101,0.8,10,8,0.47,58.4 +1632,1129,0.83,10,8.3,0.49,59.6 +1636,1159,0.94,10,9.4,0.52,55.5 +1640,1188,1,10,10,0.55,54.9 +1644,1219,1.1,10,11,0.58,52.8 +1648,1250,1.2,10,12,0.61,50.7 +1652,1278,1.3,10,13,0.64,49.2 +1656,1303,1.4,10,14,0.68,48.9 +1660,1330,1.5,10,15,0.70,46.9 +1664,1363,1.5,10,15,0.73,48.4 +1668,1392,1.6,10,16,0.76,47.6 +1672,1418,1.8,10,18,0.80,44.4 +1676,1445,1.9,10,19,0.83,43.4 +1680,1473,2,10,20,0.85,42.6 +1684,1496,2.1,10,21,0.89,42.3 +1688,1519,2.2,10,22,0.92,41.9 +1692,1542,2.3,10,23,0.95,41.2 +1696,1571,2.43,10,24.3,0.98,40.1 +1700,1596,2.7,10,27,0.97,36.0 +1704,1622,2.8,10,28,1.05,37.4 +1708,1650,2.9,10,29,1.08,37.1 +1712,1674,3.1,10,31,1.11,35.8 +1716,1696,3.3,10,33,1.17,35.3 +1720,1720,3.4,10,34,1.18,34.8 +1724,1747,3.5,10,35,1.21,34.5 +1728,1765,3.7,10,37,1.24,33.5 +1732,1793,3.84,10,38.4,1.27,33.2 +1736,1819,4,10,40,1.31,32.8 +1740,1833,4.2,10,42,1.36,32.3 +1744,1860,4.4,10,44,1.41,32.1 +1748,1889,4.5,10,45,1.43,31.8 +1752,1911,4.7,10,47,1.48,31.5 +1756,1933,4.9,10,49,1.52,31.1 +1760,1962,5.1,10,51,1.55,30.3 +1764,1984,5.3,10,53,1.58,29.8 +1768,2004,5.5,10,55,1.62,29.5 +1772,2027,5.7,10,57,1.66,29.0 +1776,2052,5.9,10,59,1.69,28.7 +1780,2075,6.09,10,60.9,1.73,28.4 +1784,2093,6.3,10,63,1.77,28.2 +1788,2118,6.5,10,65,1.80,27.7 +1792,2140,6.73,10,67.3,1.85,27.4 +1796,2160,6.9,10,69,1.88,27.2 +1800,2179,7.13,10,71.3,1.91,26.8 +1804,2200,7.4,10,74,1.99,26.9 +1808,2225,7.6,10,76,2.00,26.3 +1812,2245,7.9,10,79,2.06,26.1 +1816,2273,8.1,10,81,2.09,25.8 +1820,2293,8.33,10,83.3,2.12,25.5 +1824,2307,8.64,10,86.4,2.17,25.1 +1828,2333,8.89,10,88.9,2.21,24.9 +1832,2353,9.14,10,91.4,2.27,24.9 +1836,2378,9.4,10,94,2.30,24.5 +1840,2398,9.7,10,97,2.36,24.4 +1844,2410,10,10,100,2.41,24.1 +1848,2436,10.2,10,102,2.45,24.1 +1852,2453,10.5,10,105,2.48,23.6 +1856,2481,10.79,10,107.9,2.55,23.7 +1860,2499,11.04,10,110.4,2.58,23.4 +1864,2522,11.35,10,113.5,2.62,23.1 +1868,2543,11.6,10,116,2.65,22.8 +1872,2558,11.9,10,119,2.69,22.6 +1876,2575,12.28,10,122.8,2.74,22.3 +1880,2602,12.44,10,124.4,2.77,22.3 +1884,2613,12.8,10,128,2.82,22.0 +1888,2633,13.13,10,131.3,2.88,21.9 +1892,2657,13.4,10,134,2.89,21.6 +1896,2665,13.62,10,136.2,2.92,21.4 +1900,2668,13.6,10,136,2.93,21.5 diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-12V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-12V.csv index 847b36b5f..d33fb8807 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-12V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-12V.csv @@ -1,202 +1,202 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W), -1100,2976,17.03,12,204.4,-2.90,14.2, -1104,2969,17.08,12,205.0,-2.92,14.2, -1108,2971,16.76,12,201.1,-2.89,14.4, -1112,2933,16.52,12,198.2,-2.83,14.3, -1116,2916,16.08,12,193.0,-2.79,14.4, -1120,2898,15.69,12,188.3,-2.76,14.7, -1124,2874,15.31,12,183.7,-2.72,14.8, -1128,2852,15,12,180.0,-2.67,14.8, -1132,2841,14.51,12,174.1,-2.60,14.9, -1136,2817,14.17,12,170.0,-2.59,15.3, -1140,2792,13.82,12,165.8,-2.56,15.5, -1144,2773,13.46,12,161.5,-2.49,15.4, -1148,2755,13.08,12,157.0,-2.44,15.5, -1152,2728,12.8,12,153.6,-2.43,15.8, -1156,2714,12.4,12,148.8,-2.39,16.0, -1160,2689,12,12,144.0,-2.34,16.3, -1164,2676,11.66,12,139.9,-2.30,16.5, -1168,2654,11.31,12,135.7,-2.25,16.6, -1172,2615,11.1,12,133.2,-2.23,16.7, -1176,2610,10.74,12,128.9,-2.18,16.9, -1180,2576,10.5,12,126.0,-2.14,17.0, -1184,2560,10.11,12,121.3,-2.10,17.3, -1188,2533,9.84,12,118.1,-2.07,17.5, -1192,2510,9.5,12,114.0,-2.01,17.6, -1196,2482,9.2,12,110.4,-1.98,18.0, -1200,2457,8.9,12,106.8,-1.95,18.2, -1204,2443,8.6,12,103.2,-1.88,18.2, -1208,2412,8.3,12,99.6,-1.85,18.5, -1212,2383,8,12,96.0,-1.81,18.8, -1216,2365,7.7,12,92.4,-1.78,19.2, -1220,2335,7.4,12,88.8,-1.73,19.5, -1224,2318,7.1,12,85.2,-1.66,19.5, -1228,2284,6.9,12,82.8,-1.65,19.9, -1232,2260,6.6,12,79.2,-1.61,20.3, -1236,2231,6.4,12,76.8,-1.56,20.3, -1240,2208,6.2,12,74.4,-1.53,20.6, -1244,2189,5.99,12,71.9,-1.49,20.8, -1248,2159,5.77,12,69.2,-1.47,21.2, -1252,2140,5.5,12,66.0,-1.44,21.8, -1256,2106,5.32,12,63.8,-1.40,21.9, -1260,2082,5.17,12,62.0,-1.37,22.1, -1264,2057,4.9,12,58.8,-1.33,22.7, -1268,2038,4.7,12,56.4,-1.29,22.9, -1272,2006,4.56,12,54.7,-1.28,23.4, -1276,1982,4.3,12,51.6,-1.22,23.6, -1280,1955,4.1,12,49.2,-1.19,24.2, -1284,1925,3.9,12,46.8,-1.15,24.6, -1288,1898,3.73,12,44.8,-1.12,24.9, -1292,1869,3.6,12,43.2,-1.08,25.1, -1296,1846,3.4,12,40.8,-1.04,25.6, -1300,1815,3.3,12,39.6,-1.02,25.8, -1304,1788,3.1,12,37.2,-0.99,26.7, -1308,1763,2.98,12,35.8,-0.96,26.9, -1312,1737,2.8,12,33.6,-0.93,27.7, -1316,1708,2.7,12,32.4,-0.90,27.9, -1320,1681,2.41,12,28.9,-0.87,30.1, -1324,1651,2.3,12,27.6,-0.83,30.2, -1328,1620,2.1,12,25.2,-0.79,31.5, -1332,1588,2,12,24.0,-0.77,32.1, -1336,1557,1.9,12,22.8,-0.74,32.4, -1340,1529,1.8,12,21.6,-0.72,33.4, -1344,1504,1.7,12,20.4,-0.69,34.0, -1348,1470,1.6,12,19.2,-0.66,34.3, -1352,1439,1.5,12,18.0,-0.64,35.5, -1356,1411,1.31,12,15.7,-0.60,38.4, -1360,1375,1.3,12,15.6,-0.57,36.6, -1364,1338,1.2,12,14.4,-0.54,37.8, -1368,1309,1.1,12,13.2,-0.52,39.2, -1372,1276,1,12,12.0,-0.49,41.2, -1376,1246,0.9,12,10.8,-0.47,43.7, -1380,1211,0.8,12,9.6,-0.44,45.8, -1384,1169,0.8,12,9.6,-0.42,43.5, -1388,1134,0.7,12,8.4,-0.39,46.4, -1392,1099,0.6,12,7.2,-0.37,51.0, -1396,1057,0.5,12,6.0,-0.34,56.7, -1400,1023,0.5,12,6.0,-0.32,52.9, -1404,986,0.41,12,4.9,-0.29,59.9, -1408,944,0.4,12,4.8,-0.27,55.8, -1412,903,0.4,12,4.8,-0.24,51.0, -1416,865,0.3,12,3.6,-0.23,63.0, -1420,820,0.29,12,3.5,-0.20,58.7, -1424,777,0.2,12,2.4,-0.18,73.7, -1428,737,0.2,12,2.4,-0.16,68.0, -1432,690,0.2,12,2.4,-0.15,60.5, -1436,643,0.1,12,1.2,-0.12,102.1, -1440,597,0.1,12,1.2,-0.11,90.7, -1444,549,0.1,12,1.2,-0.09,75.6, -1448,499,0.05,12,0.6,-0.07,121.0, -1452,448,0.05,12,0.6,-0.06,98.3, -1456,400,0.05,12,0.6,-0.05,75.6, -1460,346,0.05,12,0.6,-0.04,60.5, -1464,0,0,12,0.0,0.00,0.0, -1468,0,0,12,0.0,0.00,0.0, -1472,0,0,12,0.0,0.00,0.0, -1476,0,0,12,0.0,0.00,0.0, -1480,0,0,12,0.0,0.00,0.0, -1484,0,0,12,0.0,0.00,0.0, -1488,0,0,12,0.0,0.00,0.0, -1492,0,0,12,0.0,0.00,0.0, -1496,0,0,12,0.0,0.00,0.0, -1500,0,0,12,0.0,0.00,0.0, -1504,0,0,12,0.0,0.00,0.0, -1508,0,0,12,0.0,0.00,0.0, -1512,0,0,12,0.0,0.00,0.0, -1516,0,0,12,0.0,0.00,0.0, -1520,0,0,12,0.0,0.00,0.0, -1524,0,0,12,0.0,0.00,0.0, -1528,0,0,12,0.0,0.00,0.0, -1532,0,0,12,0.0,0.00,0.0, -1536,0,0,12,0.0,0.00,0.0, -1540,342,0.05,12,0.6,0.04,68.0, -1544,395,0.05,12,0.6,0.05,90.7, -1548,444,0.05,12,0.6,0.07,121.0, -1552,494,0.1,12,1.2,0.10,79.4, -1556,542,0.1,12,1.2,0.11,94.5, -1560,592,0.1,12,1.2,0.13,109.6, -1564,634,0.1,12,1.2,0.15,128.5, -1568,680,0.2,12,2.4,0.18,73.7, -1572,726,0.2,12,2.4,0.20,83.2, -1576,768,0.2,12,2.4,0.22,92.6, -1580,812,0.3,12,3.6,0.25,70.6, -1584,851,0.3,12,3.6,0.28,76.9, -1588,892,0.4,12,4.8,0.31,64.3, -1592,931,0.4,12,4.8,0.33,69.0, -1596,971,0.5,12,6.0,0.37,61.2, -1600,1009,0.5,12,6.0,0.39,65.8, -1604,1044,0.6,12,7.2,0.43,59.2, -1608,1084,0.7,12,8.4,0.46,54.5, -1612,1121,0.7,12,8.4,0.49,58.3, -1616,1152,0.8,12,9.6,0.52,53.9, -1620,1186,0.8,12,9.6,0.55,57.2, -1624,1227,1,12,12.0,0.59,49.1, -1628,1263,1,12,12.0,0.63,52.2, -1632,1291,1.1,12,13.2,0.65,49.5, -1636,1322,1.2,12,14.4,0.68,47.6, -1640,1354,1.3,12,15.6,0.71,45.6, -1644,1385,1.4,12,16.8,0.76,45.1, -1648,1424,1.5,12,18.0,0.79,43.8, -1652,1453,1.6,12,19.2,0.83,43.2, -1656,1485,1.7,12,20.4,0.86,42.0, -1660,1515,1.8,12,21.6,0.89,41.4, -1664,1539,2,12,24.0,0.93,38.7, -1668,1567,2.1,12,25.2,0.97,38.3, -1672,1599,2.2,12,26.4,1.00,38.0, -1676,1632,2.3,12,27.6,1.04,37.6, -1680,1653,2.5,12,30.0,1.08,36.1, -1684,1686,2.8,12,33.6,1.14,33.9, -1688,1716,2.9,12,34.8,1.16,33.4, -1692,1741,3,12,36.0,1.20,33.3, -1696,1769,3.2,12,38.4,1.23,32.1, -1700,1796,3.3,12,39.6,1.28,32.3, -1704,1824,3.5,12,42.0,1.31,31.2, -1708,1854,3.67,12,44.0,1.35,30.7, -1712,1880,3.8,12,45.6,1.40,30.7, -1716,1911,4,12,48.0,1.43,29.9, -1720,1931,4.2,12,50.4,1.48,29.4, -1724,1964,4.4,12,52.8,1.53,29.0, -1728,1996,4.6,12,55.2,1.56,28.3, -1732,2017,4.8,12,57.6,1.63,28.3, -1736,2046,5,12,60.0,1.67,27.9, -1740,2070,5.2,12,62.4,1.71,27.4, -1744,2083,5.4,12,64.8,1.77,27.3, -1748,2118,5.69,12,68.3,1.82,26.6, -1752,2143,5.8,12,69.6,1.85,26.5, -1756,2169,6,12,72.0,1.91,26.5, -1760,2190,6.3,12,75.6,1.92,25.4, -1764,2229,6.5,12,78.0,1.96,25.2, -1768,2244,6.7,12,80.4,2.03,25.3, -1772,2269,7,12,84.0,2.09,24.8, -1776,2304,7.2,12,86.4,2.13,24.7, -1780,2330,7.5,12,90.0,2.18,24.2, -1784,2345,7.8,12,93.6,2.24,23.9, -1788,2391,8,12,96.0,2.27,23.7, -1792,2411,8.32,12,99.8,2.33,23.4, -1796,2432,8.64,12,103.7,2.40,23.1, -1800,2456,8.9,12,106.8,2.46,23.1, -1804,2482,9.24,12,110.9,2.51,22.6, -1808,2508,9.5,12,114.0,2.56,22.5, -1812,2535,9.82,12,117.8,2.62,22.2, -1816,2562,10.14,12,121.7,2.65,21.8, -1820,2585,10.45,12,125.4,2.71,21.6, -1824,2613,10.72,12,128.6,2.78,21.6, -1828,2620,11.1,12,133.2,2.84,21.4, -1832,2661,11.32,12,135.8,2.87,21.1, -1836,2681,11.62,12,139.4,2.93,21.0, -1840,2698,12.01,12,144.1,3.01,20.9, -1844,2721,12.37,12,148.4,3.04,20.5, -1848,2751,12.61,12,151.3,3.08,20.4, -1852,2773,13.04,12,156.5,3.16,20.2, -1856,2782,13.44,12,161.3,3.23,20.0, -1860,2808,13.7,12,164.4,3.26,19.8, -1864,2835,14.11,12,169.3,3.32,19.6, -1868,2859,14.4,12,172.8,3.38,19.6, -1872,2879,14.76,12,177.1,3.40,19.2, -1876,2904,15.13,12,181.6,3.45,19.0, -1880,2923,15.52,12,186.2,3.50,18.8, -1884,2937,15.87,12,190.4,3.57,18.7, -1888,2962,16.3,12,195.6,3.64,18.6, -1892,2976,16.74,12,200.9,3.71,18.4, -1896,2994,16.86,12,202.3,3.69,18.2, -1900,2995,16.91,12,202.9,3.71,18.3, + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W), +1100,2976,17.03,12,204.4,-2.90,14.2, +1104,2969,17.08,12,205.0,-2.92,14.2, +1108,2971,16.76,12,201.1,-2.89,14.4, +1112,2933,16.52,12,198.2,-2.83,14.3, +1116,2916,16.08,12,193.0,-2.79,14.4, +1120,2898,15.69,12,188.3,-2.76,14.7, +1124,2874,15.31,12,183.7,-2.72,14.8, +1128,2852,15,12,180.0,-2.67,14.8, +1132,2841,14.51,12,174.1,-2.60,14.9, +1136,2817,14.17,12,170.0,-2.59,15.3, +1140,2792,13.82,12,165.8,-2.56,15.5, +1144,2773,13.46,12,161.5,-2.49,15.4, +1148,2755,13.08,12,157.0,-2.44,15.5, +1152,2728,12.8,12,153.6,-2.43,15.8, +1156,2714,12.4,12,148.8,-2.39,16.0, +1160,2689,12,12,144.0,-2.34,16.3, +1164,2676,11.66,12,139.9,-2.30,16.5, +1168,2654,11.31,12,135.7,-2.25,16.6, +1172,2615,11.1,12,133.2,-2.23,16.7, +1176,2610,10.74,12,128.9,-2.18,16.9, +1180,2576,10.5,12,126.0,-2.14,17.0, +1184,2560,10.11,12,121.3,-2.10,17.3, +1188,2533,9.84,12,118.1,-2.07,17.5, +1192,2510,9.5,12,114.0,-2.01,17.6, +1196,2482,9.2,12,110.4,-1.98,18.0, +1200,2457,8.9,12,106.8,-1.95,18.2, +1204,2443,8.6,12,103.2,-1.88,18.2, +1208,2412,8.3,12,99.6,-1.85,18.5, +1212,2383,8,12,96.0,-1.81,18.8, +1216,2365,7.7,12,92.4,-1.78,19.2, +1220,2335,7.4,12,88.8,-1.73,19.5, +1224,2318,7.1,12,85.2,-1.66,19.5, +1228,2284,6.9,12,82.8,-1.65,19.9, +1232,2260,6.6,12,79.2,-1.61,20.3, +1236,2231,6.4,12,76.8,-1.56,20.3, +1240,2208,6.2,12,74.4,-1.53,20.6, +1244,2189,5.99,12,71.9,-1.49,20.8, +1248,2159,5.77,12,69.2,-1.47,21.2, +1252,2140,5.5,12,66.0,-1.44,21.8, +1256,2106,5.32,12,63.8,-1.40,21.9, +1260,2082,5.17,12,62.0,-1.37,22.1, +1264,2057,4.9,12,58.8,-1.33,22.7, +1268,2038,4.7,12,56.4,-1.29,22.9, +1272,2006,4.56,12,54.7,-1.28,23.4, +1276,1982,4.3,12,51.6,-1.22,23.6, +1280,1955,4.1,12,49.2,-1.19,24.2, +1284,1925,3.9,12,46.8,-1.15,24.6, +1288,1898,3.73,12,44.8,-1.12,24.9, +1292,1869,3.6,12,43.2,-1.08,25.1, +1296,1846,3.4,12,40.8,-1.04,25.6, +1300,1815,3.3,12,39.6,-1.02,25.8, +1304,1788,3.1,12,37.2,-0.99,26.7, +1308,1763,2.98,12,35.8,-0.96,26.9, +1312,1737,2.8,12,33.6,-0.93,27.7, +1316,1708,2.7,12,32.4,-0.90,27.9, +1320,1681,2.41,12,28.9,-0.87,30.1, +1324,1651,2.3,12,27.6,-0.83,30.2, +1328,1620,2.1,12,25.2,-0.79,31.5, +1332,1588,2,12,24.0,-0.77,32.1, +1336,1557,1.9,12,22.8,-0.74,32.4, +1340,1529,1.8,12,21.6,-0.72,33.4, +1344,1504,1.7,12,20.4,-0.69,34.0, +1348,1470,1.6,12,19.2,-0.66,34.3, +1352,1439,1.5,12,18.0,-0.64,35.5, +1356,1411,1.31,12,15.7,-0.60,38.4, +1360,1375,1.3,12,15.6,-0.57,36.6, +1364,1338,1.2,12,14.4,-0.54,37.8, +1368,1309,1.1,12,13.2,-0.52,39.2, +1372,1276,1,12,12.0,-0.49,41.2, +1376,1246,0.9,12,10.8,-0.47,43.7, +1380,1211,0.8,12,9.6,-0.44,45.8, +1384,1169,0.8,12,9.6,-0.42,43.5, +1388,1134,0.7,12,8.4,-0.39,46.4, +1392,1099,0.6,12,7.2,-0.37,51.0, +1396,1057,0.5,12,6.0,-0.34,56.7, +1400,1023,0.5,12,6.0,-0.32,52.9, +1404,986,0.41,12,4.9,-0.29,59.9, +1408,944,0.4,12,4.8,-0.27,55.8, +1412,903,0.4,12,4.8,-0.24,51.0, +1416,865,0.3,12,3.6,-0.23,63.0, +1420,820,0.29,12,3.5,-0.20,58.7, +1424,777,0.2,12,2.4,-0.18,73.7, +1428,737,0.2,12,2.4,-0.16,68.0, +1432,690,0.2,12,2.4,-0.15,60.5, +1436,643,0.1,12,1.2,-0.12,102.1, +1440,597,0.1,12,1.2,-0.11,90.7, +1444,549,0.1,12,1.2,-0.09,75.6, +1448,499,0.05,12,0.6,-0.07,121.0, +1452,448,0.05,12,0.6,-0.06,98.3, +1456,400,0.05,12,0.6,-0.05,75.6, +1460,346,0.05,12,0.6,-0.04,60.5, +1464,0,0,12,0.0,0.00,0.0, +1468,0,0,12,0.0,0.00,0.0, +1472,0,0,12,0.0,0.00,0.0, +1476,0,0,12,0.0,0.00,0.0, +1480,0,0,12,0.0,0.00,0.0, +1484,0,0,12,0.0,0.00,0.0, +1488,0,0,12,0.0,0.00,0.0, +1492,0,0,12,0.0,0.00,0.0, +1496,0,0,12,0.0,0.00,0.0, +1500,0,0,12,0.0,0.00,0.0, +1504,0,0,12,0.0,0.00,0.0, +1508,0,0,12,0.0,0.00,0.0, +1512,0,0,12,0.0,0.00,0.0, +1516,0,0,12,0.0,0.00,0.0, +1520,0,0,12,0.0,0.00,0.0, +1524,0,0,12,0.0,0.00,0.0, +1528,0,0,12,0.0,0.00,0.0, +1532,0,0,12,0.0,0.00,0.0, +1536,0,0,12,0.0,0.00,0.0, +1540,342,0.05,12,0.6,0.04,68.0, +1544,395,0.05,12,0.6,0.05,90.7, +1548,444,0.05,12,0.6,0.07,121.0, +1552,494,0.1,12,1.2,0.10,79.4, +1556,542,0.1,12,1.2,0.11,94.5, +1560,592,0.1,12,1.2,0.13,109.6, +1564,634,0.1,12,1.2,0.15,128.5, +1568,680,0.2,12,2.4,0.18,73.7, +1572,726,0.2,12,2.4,0.20,83.2, +1576,768,0.2,12,2.4,0.22,92.6, +1580,812,0.3,12,3.6,0.25,70.6, +1584,851,0.3,12,3.6,0.28,76.9, +1588,892,0.4,12,4.8,0.31,64.3, +1592,931,0.4,12,4.8,0.33,69.0, +1596,971,0.5,12,6.0,0.37,61.2, +1600,1009,0.5,12,6.0,0.39,65.8, +1604,1044,0.6,12,7.2,0.43,59.2, +1608,1084,0.7,12,8.4,0.46,54.5, +1612,1121,0.7,12,8.4,0.49,58.3, +1616,1152,0.8,12,9.6,0.52,53.9, +1620,1186,0.8,12,9.6,0.55,57.2, +1624,1227,1,12,12.0,0.59,49.1, +1628,1263,1,12,12.0,0.63,52.2, +1632,1291,1.1,12,13.2,0.65,49.5, +1636,1322,1.2,12,14.4,0.68,47.6, +1640,1354,1.3,12,15.6,0.71,45.6, +1644,1385,1.4,12,16.8,0.76,45.1, +1648,1424,1.5,12,18.0,0.79,43.8, +1652,1453,1.6,12,19.2,0.83,43.2, +1656,1485,1.7,12,20.4,0.86,42.0, +1660,1515,1.8,12,21.6,0.89,41.4, +1664,1539,2,12,24.0,0.93,38.7, +1668,1567,2.1,12,25.2,0.97,38.3, +1672,1599,2.2,12,26.4,1.00,38.0, +1676,1632,2.3,12,27.6,1.04,37.6, +1680,1653,2.5,12,30.0,1.08,36.1, +1684,1686,2.8,12,33.6,1.14,33.9, +1688,1716,2.9,12,34.8,1.16,33.4, +1692,1741,3,12,36.0,1.20,33.3, +1696,1769,3.2,12,38.4,1.23,32.1, +1700,1796,3.3,12,39.6,1.28,32.3, +1704,1824,3.5,12,42.0,1.31,31.2, +1708,1854,3.67,12,44.0,1.35,30.7, +1712,1880,3.8,12,45.6,1.40,30.7, +1716,1911,4,12,48.0,1.43,29.9, +1720,1931,4.2,12,50.4,1.48,29.4, +1724,1964,4.4,12,52.8,1.53,29.0, +1728,1996,4.6,12,55.2,1.56,28.3, +1732,2017,4.8,12,57.6,1.63,28.3, +1736,2046,5,12,60.0,1.67,27.9, +1740,2070,5.2,12,62.4,1.71,27.4, +1744,2083,5.4,12,64.8,1.77,27.3, +1748,2118,5.69,12,68.3,1.82,26.6, +1752,2143,5.8,12,69.6,1.85,26.5, +1756,2169,6,12,72.0,1.91,26.5, +1760,2190,6.3,12,75.6,1.92,25.4, +1764,2229,6.5,12,78.0,1.96,25.2, +1768,2244,6.7,12,80.4,2.03,25.3, +1772,2269,7,12,84.0,2.09,24.8, +1776,2304,7.2,12,86.4,2.13,24.7, +1780,2330,7.5,12,90.0,2.18,24.2, +1784,2345,7.8,12,93.6,2.24,23.9, +1788,2391,8,12,96.0,2.27,23.7, +1792,2411,8.32,12,99.8,2.33,23.4, +1796,2432,8.64,12,103.7,2.40,23.1, +1800,2456,8.9,12,106.8,2.46,23.1, +1804,2482,9.24,12,110.9,2.51,22.6, +1808,2508,9.5,12,114.0,2.56,22.5, +1812,2535,9.82,12,117.8,2.62,22.2, +1816,2562,10.14,12,121.7,2.65,21.8, +1820,2585,10.45,12,125.4,2.71,21.6, +1824,2613,10.72,12,128.6,2.78,21.6, +1828,2620,11.1,12,133.2,2.84,21.4, +1832,2661,11.32,12,135.8,2.87,21.1, +1836,2681,11.62,12,139.4,2.93,21.0, +1840,2698,12.01,12,144.1,3.01,20.9, +1844,2721,12.37,12,148.4,3.04,20.5, +1848,2751,12.61,12,151.3,3.08,20.4, +1852,2773,13.04,12,156.5,3.16,20.2, +1856,2782,13.44,12,161.3,3.23,20.0, +1860,2808,13.7,12,164.4,3.26,19.8, +1864,2835,14.11,12,169.3,3.32,19.6, +1868,2859,14.4,12,172.8,3.38,19.6, +1872,2879,14.76,12,177.1,3.40,19.2, +1876,2904,15.13,12,181.6,3.45,19.0, +1880,2923,15.52,12,186.2,3.50,18.8, +1884,2937,15.87,12,190.4,3.57,18.7, +1888,2962,16.3,12,195.6,3.64,18.6, +1892,2976,16.74,12,200.9,3.71,18.4, +1896,2994,16.86,12,202.3,3.69,18.2, +1900,2995,16.91,12,202.9,3.71,18.3, diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-14V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-14V.csv index 6689d4df1..de7294093 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-14V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-14V.csv @@ -1,202 +1,202 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) -1100,2662,13.62,10,136.2,-2.31,17.0 -1104,2667,13.63,10,136.3,-2.30,16.9 -1108,2661,13.45,10,134.5,-2.30,17.1 -1112,2627,13.2,10,132,-2.28,17.3 -1116,2617,12.81,10,128.1,-2.22,17.4 -1120,2600,12.5,10,125,-2.18,17.5 -1124,2590,12.14,10,121.4,-2.16,17.8 -1128,2562,11.96,10,119.6,-2.13,17.8 -1132,2535,11.63,10,116.3,-2.11,18.1 -1136,2528,11.32,10,113.2,-2.05,18.2 -1140,2514,11,10,110,-2.04,18.6 -1144,2484,10.8,10,108,-1.99,18.4 -1148,2472,10.45,10,104.5,-1.95,18.7 -1152,2449,10.2,10,102,-1.93,18.9 -1156,2436,9.9,10,99,-1.89,19.1 -1160,2416,9.56,10,95.6,-1.84,19.2 -1164,2395,9.29,10,92.9,-1.81,19.5 -1168,2371,9.1,10,91,-1.79,19.7 -1172,2344,8.8,10,88,-1.76,19.9 -1176,2332,8.5,10,85,-1.71,20.2 -1180,2306,8.3,10,83,-1.68,20.3 -1184,2283,8.05,10,80.5,-1.65,20.5 -1188,2271,7.71,10,77.1,-1.62,21.0 -1192,2241,7.57,10,75.7,-1.59,21.0 -1196,2219,7.3,10,73,-1.56,21.3 -1200,2198,7.1,10,71,-1.54,21.7 -1204,2177,6.82,10,68.2,-1.50,22.0 -1208,2157,6.6,10,66,-1.47,22.3 -1212,2129,6.48,10,64.8,-1.44,22.3 -1216,2106,6.2,10,62,-1.41,22.8 -1220,2094,6,10,60,-1.36,22.7 -1224,2073,5.8,10,58,-1.33,22.9 -1228,2046,5.6,10,56,-1.32,23.6 -1232,2024,5.4,10,54,-1.29,23.9 -1236,2005,5.2,10,52,-1.26,24.2 -1240,1982,5,10,50,-1.22,24.4 -1244,1957,4.84,10,48.4,-1.19,24.6 -1248,1936,4.64,10,46.4,-1.17,25.1 -1252,1907,4.5,10,45,-1.14,25.3 -1256,1885,4.3,10,43,-1.10,25.5 -1260,1864,4.1,10,41,-1.08,26.2 -1264,1834,3.9,10,39,-1.06,27.1 -1268,1816,3.73,10,37.3,-1.02,27.2 -1272,1789,3.6,10,36,-0.99,27.5 -1276,1762,3.5,10,35,-0.97,27.7 -1280,1745,3.3,10,33,-0.93,28.2 -1284,1717,3.2,10,32,-0.91,28.3 -1288,1697,3,10,30,-0.88,29.3 -1292,1671,2.9,10,29,-0.85,29.4 -1296,1643,2.8,10,28,-0.83,29.5 -1300,1615,2.6,10,26,-0.80,30.7 -1304,1591,2.4,10,24,-0.78,32.5 -1308,1562,2.3,10,23,-0.75,32.7 -1312,1539,2.1,10,21,-0.72,34.3 -1316,1516,2,10,20,-0.70,34.9 -1320,1488,1.9,10,19,-0.67,35.3 -1324,1461,1.8,10,18,-0.65,36.3 -1328,1440,1.7,10,17,-0.63,37.1 -1332,1411,1.6,10,16,-0.61,38.3 -1336,1382,1.5,10,15,-0.58,38.7 -1340,1347,1.4,10,14,-0.55,39.5 -1344,1320,1.3,10,13,-0.53,40.8 -1348,1294,1.2,10,12,-0.51,42.7 -1352,1265,1.2,10,12,-0.49,40.4 -1356,1239,1.1,10,11,-0.47,42.5 -1360,1200,1,10,10,-0.44,44.5 -1364,1174,0.9,10,9,-0.42,46.9 -1368,1142,0.8,10,8,-0.40,49.9 -1372,1114,0.8,10,8,-0.38,47.6 -1376,1080,0.7,10,7,-0.35,50.5 -1380,1048,0.7,10,7,-0.34,48.0 -1384,1016,0.6,10,6,-0.31,52.2 -1388,986,0.5,10,5,-0.29,59.0 -1392,948,0.5,10,5,-0.28,55.3 -1396,916,0.4,10,4,-0.25,62.4 -1400,883,0.4,10,4,-0.24,59.0 -1404,845,0.4,10,4,-0.21,53.3 -1408,813,0.3,10,3,-0.20,66.5 -1412,774,0.22,10,2.2,-0.18,82.5 -1416,740,0.2,10,2,-0.16,81.6 -1420,701,0.2,10,2,-0.15,77.1 -1424,664,0.2,10,2,-0.14,68.0 -1428,624,0.2,10,2,-0.12,59.0 -1432,584,0.1,10,1,-0.10,99.8 -1436,545,0.1,10,1,-0.09,86.2 -1440,504,0.1,10,1,-0.07,72.6 -1444,462,0.05,10,0.5,-0.06,127.0 -1448,421,0.05,10,0.5,-0.05,108.9 -1452,377,0.05,10,0.5,-0.05,90.7 -1456,335,0.05,10,0.5,-0.03,63.5 -1460,0,0,10,0,0.00,0.0 -1464,0,0,10,0,0.00,0.0 -1468,0,0,10,0,0.00,0.0 -1472,0,0,10,0,0.00,0.0 -1476,0,0,10,0,0.00,0.0 -1480,0,0,10,0,0.00,0.0 -1484,0,0,10,0,0.00,0.0 -1488,0,0,10,0,0.00,0.0 -1492,0,0,10,0,0.00,0.0 -1496,0,0,10,0,0.00,0.0 -1500,0,0,10,0,0.00,0.0 -1504,0,0,10,0,0.00,0.0 -1508,0,0,10,0,0.00,0.0 -1512,0,0,10,0,0.00,0.0 -1516,0,0,10,0,0.00,0.0 -1520,0,0,10,0,0.00,0.0 -1524,0,0,10,0,0.00,0.0 -1528,0,0,10,0,0.00,0.0 -1532,0,0,10,0,0.00,0.0 -1536,0,0,10,0,0.00,0.0 -1540,0,0,10,0,0.00,0.0 -1544,332,0.05,10,0.5,0.04,81.6 -1548,373,0.05,10,0.5,0.05,108.9 -1552,417,0.05,10,0.5,0.07,136.1 -1556,459,0.1,10,1,0.08,77.1 -1560,500,0.1,10,1,0.09,90.7 -1564,540,0.1,10,1,0.11,108.9 -1568,582,0.1,10,1,0.13,127.0 -1572,621,0.2,10,2,0.15,72.6 -1576,657,0.2,10,2,0.16,81.6 -1580,695,0.2,10,2,0.18,90.7 -1584,733,0.2,10,2,0.20,102.1 -1588,766,0.3,10,3,0.22,74.1 -1592,805,0.3,10,3,0.24,81.6 -1596,836,0.4,10,4,0.26,65.8 -1600,873,0.4,10,4,0.29,72.6 -1604,906,0.47,10,4.7,0.31,66.6 -1608,938,0.5,10,5,0.34,67.1 -1612,974,0.5,10,5,0.36,72.6 -1616,1007,0.6,10,6,0.39,64.3 -1620,1035,0.7,10,7,0.41,59.0 -1624,1066,0.7,10,7,0.44,63.5 -1628,1101,0.8,10,8,0.47,58.4 -1632,1129,0.83,10,8.3,0.49,59.6 -1636,1159,0.94,10,9.4,0.52,55.5 -1640,1188,1,10,10,0.55,54.9 -1644,1219,1.1,10,11,0.58,52.8 -1648,1250,1.2,10,12,0.61,50.7 -1652,1278,1.3,10,13,0.64,49.2 -1656,1303,1.4,10,14,0.68,48.9 -1660,1330,1.5,10,15,0.70,46.9 -1664,1363,1.5,10,15,0.73,48.4 -1668,1392,1.6,10,16,0.76,47.6 -1672,1418,1.8,10,18,0.80,44.4 -1676,1445,1.9,10,19,0.83,43.4 -1680,1473,2,10,20,0.85,42.6 -1684,1496,2.1,10,21,0.89,42.3 -1688,1519,2.2,10,22,0.92,41.9 -1692,1542,2.3,10,23,0.95,41.2 -1696,1571,2.43,10,24.3,0.98,40.1 -1700,1596,2.7,10,27,0.97,36.0 -1704,1622,2.8,10,28,1.05,37.4 -1708,1650,2.9,10,29,1.08,37.1 -1712,1674,3.1,10,31,1.11,35.8 -1716,1696,3.3,10,33,1.17,35.3 -1720,1720,3.4,10,34,1.18,34.8 -1724,1747,3.5,10,35,1.21,34.5 -1728,1765,3.7,10,37,1.24,33.5 -1732,1793,3.84,10,38.4,1.27,33.2 -1736,1819,4,10,40,1.31,32.8 -1740,1833,4.2,10,42,1.36,32.3 -1744,1860,4.4,10,44,1.41,32.1 -1748,1889,4.5,10,45,1.43,31.8 -1752,1911,4.7,10,47,1.48,31.5 -1756,1933,4.9,10,49,1.52,31.1 -1760,1962,5.1,10,51,1.55,30.3 -1764,1984,5.3,10,53,1.58,29.8 -1768,2004,5.5,10,55,1.62,29.5 -1772,2027,5.7,10,57,1.66,29.0 -1776,2052,5.9,10,59,1.69,28.7 -1780,2075,6.09,10,60.9,1.73,28.4 -1784,2093,6.3,10,63,1.77,28.2 -1788,2118,6.5,10,65,1.80,27.7 -1792,2140,6.73,10,67.3,1.85,27.4 -1796,2160,6.9,10,69,1.88,27.2 -1800,2179,7.13,10,71.3,1.91,26.8 -1804,2200,7.4,10,74,1.99,26.9 -1808,2225,7.6,10,76,2.00,26.3 -1812,2245,7.9,10,79,2.06,26.1 -1816,2273,8.1,10,81,2.09,25.8 -1820,2293,8.33,10,83.3,2.12,25.5 -1824,2307,8.64,10,86.4,2.17,25.1 -1828,2333,8.89,10,88.9,2.21,24.9 -1832,2353,9.14,10,91.4,2.27,24.9 -1836,2378,9.4,10,94,2.30,24.5 -1840,2398,9.7,10,97,2.36,24.4 -1844,2410,10,10,100,2.41,24.1 -1848,2436,10.2,10,102,2.45,24.1 -1852,2453,10.5,10,105,2.48,23.6 -1856,2481,10.79,10,107.9,2.55,23.7 -1860,2499,11.04,10,110.4,2.58,23.4 -1864,2522,11.35,10,113.5,2.62,23.1 -1868,2543,11.6,10,116,2.65,22.8 -1872,2558,11.9,10,119,2.69,22.6 -1876,2575,12.28,10,122.8,2.74,22.3 -1880,2602,12.44,10,124.4,2.77,22.3 -1884,2613,12.8,10,128,2.82,22.0 -1888,2633,13.13,10,131.3,2.88,21.9 -1892,2657,13.4,10,134,2.89,21.6 -1896,2665,13.62,10,136.2,2.92,21.4 -1900,2668,13.6,10,136,2.93,21.5 + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) +1100,2662,13.62,10,136.2,-2.31,17.0 +1104,2667,13.63,10,136.3,-2.30,16.9 +1108,2661,13.45,10,134.5,-2.30,17.1 +1112,2627,13.2,10,132,-2.28,17.3 +1116,2617,12.81,10,128.1,-2.22,17.4 +1120,2600,12.5,10,125,-2.18,17.5 +1124,2590,12.14,10,121.4,-2.16,17.8 +1128,2562,11.96,10,119.6,-2.13,17.8 +1132,2535,11.63,10,116.3,-2.11,18.1 +1136,2528,11.32,10,113.2,-2.05,18.2 +1140,2514,11,10,110,-2.04,18.6 +1144,2484,10.8,10,108,-1.99,18.4 +1148,2472,10.45,10,104.5,-1.95,18.7 +1152,2449,10.2,10,102,-1.93,18.9 +1156,2436,9.9,10,99,-1.89,19.1 +1160,2416,9.56,10,95.6,-1.84,19.2 +1164,2395,9.29,10,92.9,-1.81,19.5 +1168,2371,9.1,10,91,-1.79,19.7 +1172,2344,8.8,10,88,-1.76,19.9 +1176,2332,8.5,10,85,-1.71,20.2 +1180,2306,8.3,10,83,-1.68,20.3 +1184,2283,8.05,10,80.5,-1.65,20.5 +1188,2271,7.71,10,77.1,-1.62,21.0 +1192,2241,7.57,10,75.7,-1.59,21.0 +1196,2219,7.3,10,73,-1.56,21.3 +1200,2198,7.1,10,71,-1.54,21.7 +1204,2177,6.82,10,68.2,-1.50,22.0 +1208,2157,6.6,10,66,-1.47,22.3 +1212,2129,6.48,10,64.8,-1.44,22.3 +1216,2106,6.2,10,62,-1.41,22.8 +1220,2094,6,10,60,-1.36,22.7 +1224,2073,5.8,10,58,-1.33,22.9 +1228,2046,5.6,10,56,-1.32,23.6 +1232,2024,5.4,10,54,-1.29,23.9 +1236,2005,5.2,10,52,-1.26,24.2 +1240,1982,5,10,50,-1.22,24.4 +1244,1957,4.84,10,48.4,-1.19,24.6 +1248,1936,4.64,10,46.4,-1.17,25.1 +1252,1907,4.5,10,45,-1.14,25.3 +1256,1885,4.3,10,43,-1.10,25.5 +1260,1864,4.1,10,41,-1.08,26.2 +1264,1834,3.9,10,39,-1.06,27.1 +1268,1816,3.73,10,37.3,-1.02,27.2 +1272,1789,3.6,10,36,-0.99,27.5 +1276,1762,3.5,10,35,-0.97,27.7 +1280,1745,3.3,10,33,-0.93,28.2 +1284,1717,3.2,10,32,-0.91,28.3 +1288,1697,3,10,30,-0.88,29.3 +1292,1671,2.9,10,29,-0.85,29.4 +1296,1643,2.8,10,28,-0.83,29.5 +1300,1615,2.6,10,26,-0.80,30.7 +1304,1591,2.4,10,24,-0.78,32.5 +1308,1562,2.3,10,23,-0.75,32.7 +1312,1539,2.1,10,21,-0.72,34.3 +1316,1516,2,10,20,-0.70,34.9 +1320,1488,1.9,10,19,-0.67,35.3 +1324,1461,1.8,10,18,-0.65,36.3 +1328,1440,1.7,10,17,-0.63,37.1 +1332,1411,1.6,10,16,-0.61,38.3 +1336,1382,1.5,10,15,-0.58,38.7 +1340,1347,1.4,10,14,-0.55,39.5 +1344,1320,1.3,10,13,-0.53,40.8 +1348,1294,1.2,10,12,-0.51,42.7 +1352,1265,1.2,10,12,-0.49,40.4 +1356,1239,1.1,10,11,-0.47,42.5 +1360,1200,1,10,10,-0.44,44.5 +1364,1174,0.9,10,9,-0.42,46.9 +1368,1142,0.8,10,8,-0.40,49.9 +1372,1114,0.8,10,8,-0.38,47.6 +1376,1080,0.7,10,7,-0.35,50.5 +1380,1048,0.7,10,7,-0.34,48.0 +1384,1016,0.6,10,6,-0.31,52.2 +1388,986,0.5,10,5,-0.29,59.0 +1392,948,0.5,10,5,-0.28,55.3 +1396,916,0.4,10,4,-0.25,62.4 +1400,883,0.4,10,4,-0.24,59.0 +1404,845,0.4,10,4,-0.21,53.3 +1408,813,0.3,10,3,-0.20,66.5 +1412,774,0.22,10,2.2,-0.18,82.5 +1416,740,0.2,10,2,-0.16,81.6 +1420,701,0.2,10,2,-0.15,77.1 +1424,664,0.2,10,2,-0.14,68.0 +1428,624,0.2,10,2,-0.12,59.0 +1432,584,0.1,10,1,-0.10,99.8 +1436,545,0.1,10,1,-0.09,86.2 +1440,504,0.1,10,1,-0.07,72.6 +1444,462,0.05,10,0.5,-0.06,127.0 +1448,421,0.05,10,0.5,-0.05,108.9 +1452,377,0.05,10,0.5,-0.05,90.7 +1456,335,0.05,10,0.5,-0.03,63.5 +1460,0,0,10,0,0.00,0.0 +1464,0,0,10,0,0.00,0.0 +1468,0,0,10,0,0.00,0.0 +1472,0,0,10,0,0.00,0.0 +1476,0,0,10,0,0.00,0.0 +1480,0,0,10,0,0.00,0.0 +1484,0,0,10,0,0.00,0.0 +1488,0,0,10,0,0.00,0.0 +1492,0,0,10,0,0.00,0.0 +1496,0,0,10,0,0.00,0.0 +1500,0,0,10,0,0.00,0.0 +1504,0,0,10,0,0.00,0.0 +1508,0,0,10,0,0.00,0.0 +1512,0,0,10,0,0.00,0.0 +1516,0,0,10,0,0.00,0.0 +1520,0,0,10,0,0.00,0.0 +1524,0,0,10,0,0.00,0.0 +1528,0,0,10,0,0.00,0.0 +1532,0,0,10,0,0.00,0.0 +1536,0,0,10,0,0.00,0.0 +1540,0,0,10,0,0.00,0.0 +1544,332,0.05,10,0.5,0.04,81.6 +1548,373,0.05,10,0.5,0.05,108.9 +1552,417,0.05,10,0.5,0.07,136.1 +1556,459,0.1,10,1,0.08,77.1 +1560,500,0.1,10,1,0.09,90.7 +1564,540,0.1,10,1,0.11,108.9 +1568,582,0.1,10,1,0.13,127.0 +1572,621,0.2,10,2,0.15,72.6 +1576,657,0.2,10,2,0.16,81.6 +1580,695,0.2,10,2,0.18,90.7 +1584,733,0.2,10,2,0.20,102.1 +1588,766,0.3,10,3,0.22,74.1 +1592,805,0.3,10,3,0.24,81.6 +1596,836,0.4,10,4,0.26,65.8 +1600,873,0.4,10,4,0.29,72.6 +1604,906,0.47,10,4.7,0.31,66.6 +1608,938,0.5,10,5,0.34,67.1 +1612,974,0.5,10,5,0.36,72.6 +1616,1007,0.6,10,6,0.39,64.3 +1620,1035,0.7,10,7,0.41,59.0 +1624,1066,0.7,10,7,0.44,63.5 +1628,1101,0.8,10,8,0.47,58.4 +1632,1129,0.83,10,8.3,0.49,59.6 +1636,1159,0.94,10,9.4,0.52,55.5 +1640,1188,1,10,10,0.55,54.9 +1644,1219,1.1,10,11,0.58,52.8 +1648,1250,1.2,10,12,0.61,50.7 +1652,1278,1.3,10,13,0.64,49.2 +1656,1303,1.4,10,14,0.68,48.9 +1660,1330,1.5,10,15,0.70,46.9 +1664,1363,1.5,10,15,0.73,48.4 +1668,1392,1.6,10,16,0.76,47.6 +1672,1418,1.8,10,18,0.80,44.4 +1676,1445,1.9,10,19,0.83,43.4 +1680,1473,2,10,20,0.85,42.6 +1684,1496,2.1,10,21,0.89,42.3 +1688,1519,2.2,10,22,0.92,41.9 +1692,1542,2.3,10,23,0.95,41.2 +1696,1571,2.43,10,24.3,0.98,40.1 +1700,1596,2.7,10,27,0.97,36.0 +1704,1622,2.8,10,28,1.05,37.4 +1708,1650,2.9,10,29,1.08,37.1 +1712,1674,3.1,10,31,1.11,35.8 +1716,1696,3.3,10,33,1.17,35.3 +1720,1720,3.4,10,34,1.18,34.8 +1724,1747,3.5,10,35,1.21,34.5 +1728,1765,3.7,10,37,1.24,33.5 +1732,1793,3.84,10,38.4,1.27,33.2 +1736,1819,4,10,40,1.31,32.8 +1740,1833,4.2,10,42,1.36,32.3 +1744,1860,4.4,10,44,1.41,32.1 +1748,1889,4.5,10,45,1.43,31.8 +1752,1911,4.7,10,47,1.48,31.5 +1756,1933,4.9,10,49,1.52,31.1 +1760,1962,5.1,10,51,1.55,30.3 +1764,1984,5.3,10,53,1.58,29.8 +1768,2004,5.5,10,55,1.62,29.5 +1772,2027,5.7,10,57,1.66,29.0 +1776,2052,5.9,10,59,1.69,28.7 +1780,2075,6.09,10,60.9,1.73,28.4 +1784,2093,6.3,10,63,1.77,28.2 +1788,2118,6.5,10,65,1.80,27.7 +1792,2140,6.73,10,67.3,1.85,27.4 +1796,2160,6.9,10,69,1.88,27.2 +1800,2179,7.13,10,71.3,1.91,26.8 +1804,2200,7.4,10,74,1.99,26.9 +1808,2225,7.6,10,76,2.00,26.3 +1812,2245,7.9,10,79,2.06,26.1 +1816,2273,8.1,10,81,2.09,25.8 +1820,2293,8.33,10,83.3,2.12,25.5 +1824,2307,8.64,10,86.4,2.17,25.1 +1828,2333,8.89,10,88.9,2.21,24.9 +1832,2353,9.14,10,91.4,2.27,24.9 +1836,2378,9.4,10,94,2.30,24.5 +1840,2398,9.7,10,97,2.36,24.4 +1844,2410,10,10,100,2.41,24.1 +1848,2436,10.2,10,102,2.45,24.1 +1852,2453,10.5,10,105,2.48,23.6 +1856,2481,10.79,10,107.9,2.55,23.7 +1860,2499,11.04,10,110.4,2.58,23.4 +1864,2522,11.35,10,113.5,2.62,23.1 +1868,2543,11.6,10,116,2.65,22.8 +1872,2558,11.9,10,119,2.69,22.6 +1876,2575,12.28,10,122.8,2.74,22.3 +1880,2602,12.44,10,124.4,2.77,22.3 +1884,2613,12.8,10,128,2.82,22.0 +1888,2633,13.13,10,131.3,2.88,21.9 +1892,2657,13.4,10,134,2.89,21.6 +1896,2665,13.62,10,136.2,2.92,21.4 +1900,2668,13.6,10,136,2.93,21.5 diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-16V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-16V.csv index 271f69d89..a1d7dd854 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-16V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-16V.csv @@ -1,202 +1,202 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) -1100,3465,24.30,16,388.8,-4.07,10.5 -1104,3468,24.3,16,388.8,-4.05,10.4 -1108,3449,23.78,16,380.5,-4.02,10.6 -1112,3421,23.25,16,372.0,-3.96,10.7 -1116,3410,22.64,16,362.2,-3.90,10.8 -1120,3395,22.07,16,353.1,-3.87,11.0 -1124,3374,21.46,16,343.4,-3.82,11.1 -1128,3356,20.98,16,335.7,-3.80,11.3 -1132,3323,20.44,16,327.0,-3.75,11.5 -1136,3307,19.9,16,318.4,-3.71,11.7 -1140,3282,19.32,16,309.1,-3.66,11.8 -1144,3248,18.74,16,299.8,-3.59,12.0 -1148,3223,18.11,16,289.8,-3.52,12.1 -1152,3207,17.53,16,280.5,-3.45,12.3 -1156,3177,17,16,272.0,-3.40,12.5 -1160,3153,16.45,16,263.2,-3.31,12.6 -1164,3111,15.96,16,255.4,-3.25,12.7 -1168,3086,15.48,16,247.7,-3.20,12.9 -1172,3061,14.98,16,239.7,-3.11,13.0 -1176,3050,14.43,16,230.9,-3.04,13.2 -1180,3003,14.04,16,224.6,-2.99,13.3 -1184,2988,13.55,16,216.8,-2.94,13.6 -1188,2952,13.1,16,209.6,-2.86,13.7 -1192,2918,12.74,16,203.8,-2.82,13.8 -1196,2896,12.3,16,196.8,-2.77,14.1 -1200,2865,11.93,16,190.9,-2.71,14.2 -1204,2845,11.59,16,185.4,-2.66,14.3 -1208,2821,11.2,16,179.2,-2.58,14.4 -1212,2792,10.8,16,172.8,-2.55,14.8 -1216,2759,10.5,16,168.0,-2.51,14.9 -1220,2736,10.27,16,164.3,-2.45,14.9 -1224,2712,9.9,16,158.4,-2.38,15.0 -1228,2681,9.5,16,152.0,-2.35,15.5 -1232,2661,9.2,16,147.2,-2.28,15.5 -1236,2629,8.9,16,142.4,-2.24,15.7 -1240,2599,8.6,16,137.6,-2.20,16.0 -1244,2576,8.2,16,131.2,-2.12,16.1 -1248,2548,7.9,16,126.4,-2.08,16.5 -1252,2517,7.57,16,121.1,-2.02,16.7 -1256,2482,7.19,16,115.0,-1.98,17.2 -1260,2440,6.89,16,110.2,-1.94,17.6 -1264,2419,6.53,16,104.5,-1.86,17.8 -1268,2369,6.29,16,100.6,-1.81,18.0 -1272,2345,6,16,96.0,-1.76,18.4 -1276,2306,5.75,16,92.0,-1.70,18.5 -1280,2278,5.5,16,88.0,-1.67,19.0 -1284,2248,5.3,16,84.8,-1.61,19.0 -1288,2222,5,16,80.0,-1.56,19.4 -1292,2199,4.8,16,76.8,-1.51,19.7 -1296,2157,4.6,16,73.6,-1.49,20.2 -1300,2140,4.4,16,70.4,-1.44,20.4 -1304,2106,4.2,16,67.2,-1.40,20.8 -1308,2081,3.98,16,63.7,-1.35,21.2 -1312,2040,3.7,16,59.2,-1.30,22.0 -1316,2001,3.58,16,57.3,-1.26,21.9 -1320,1968,3.39,16,54.2,-1.20,22.2 -1324,1936,3.2,16,51.2,-1.16,22.7 -1328,1903,3,16,48.0,-1.12,23.3 -1332,1870,2.84,16,45.4,-1.10,24.3 -1336,1840,2.7,16,43.2,-1.05,24.4 -1340,1807,2.4,16,38.4,-1.02,26.6 -1344,1773,2.3,16,36.8,-0.98,26.6 -1348,1738,2.1,16,33.6,-0.94,28.1 -1352,1705,2,16,32.0,-0.90,28.2 -1356,1667,1.9,16,30.4,-0.87,28.6 -1360,1630,1.7,16,27.2,-0.82,30.2 -1364,1589,1.6,16,25.6,-0.78,30.7 -1368,1554,1.5,16,24.0,-0.74,31.0 -1372,1519,1.4,16,22.4,-0.72,32.0 -1376,1482,1.3,16,20.8,-0.68,32.7 -1380,1448,1.2,16,19.2,-0.65,33.8 -1384,1410,1.1,16,17.6,-0.62,35.1 -1388,1362,1,16,16.0,-0.58,36.3 -1392,1325,0.9,16,14.4,-0.54,37.5 -1396,1285,0.8,16,12.8,-0.51,40.0 -1400,1246,0.7,16,11.2,-0.48,42.9 -1404,1199,0.66,16,10.6,-0.44,42.1 -1408,1153,0.56,16,9.0,-0.42,46.6 -1412,1113,0.5,16,8.0,-0.39,48.2 -1416,1064,0.4,16,6.4,-0.35,55.3 -1420,1016,0.4,16,6.4,-0.32,50.3 -1424,970,0.37,16,5.9,-0.29,49.8 -1428,916,0.3,16,4.8,-0.26,53.9 -1432,869,0.2,16,3.2,-0.24,73.7 -1436,810,0.2,16,3.2,-0.21,65.2 -1440,758,0.2,16,3.2,-0.18,56.7 -1444,697,0.1,16,1.6,-0.15,93.6 -1448,641,0.1,16,1.6,-0.13,79.4 -1452,578,0.1,16,1.6,-0.10,65.2 -1456,513,0.05,16,0.8,-0.09,107.7 -1460,450,0.05,16,0.8,-0.07,85.0 -1464,388,0.05,16,0.8,-0.05,62.4 -1468,317,0.05,16,0.8,-0.04,45.4 -1472,0,0,16,0.0,0.00,0.0 -1476,0,0,16,0.0,0.00,0.0 -1480,0,0,16,0.0,0.00,0.0 -1484,0,0,16,0.0,0.00,0.0 -1488,0,0,16,0.0,0.00,0.0 -1492,0,0,16,0.0,0.00,0.0 -1496,0,0,16,0.0,0.00,0.0 -1500,0,0,16,0.0,0.00,0.0 -1504,0,0,16,0.0,0.00,0.0 -1508,0,0,16,0.0,0.00,0.0 -1512,0,0,16,0.0,0.00,0.0 -1516,0,0,16,0.0,0.00,0.0 -1520,0,0,16,0.0,0.00,0.0 -1524,0,0,16,0.0,0.00,0.0 -1528,0,0,16,0.0,0.00,0.0 -1532,308,0.05,16,0.8,0.04,51.0 -1536,375,0.05,16,0.8,0.05,68.0 -1540,441,0.05,16,0.8,0.08,96.4 -1544,508,0.05,16,0.8,0.10,124.7 -1548,573,0.1,16,1.6,0.13,79.4 -1552,630,0.1,16,1.6,0.15,96.4 -1556,688,0.1,16,1.6,0.18,113.4 -1560,749,0.2,16,3.2,0.22,68.0 -1564,804,0.2,16,3.2,0.25,78.0 -1568,859,0.2,16,3.2,0.29,90.7 -1572,906,0.3,16,4.8,0.32,67.1 -1576,962,0.4,16,6.4,0.36,56.0 -1580,1007,0.4,16,6.4,0.40,62.4 -1584,1057,0.5,16,8.0,0.44,54.4 -1588,1104,0.5,16,8.0,0.47,59.0 -1592,1148,0.6,16,9.6,0.51,53.4 -1596,1195,0.7,16,11.2,0.56,49.8 -1600,1237,0.8,16,12.8,0.60,47.1 -1604,1279,0.8,16,12.8,0.64,50.0 -1608,1319,0.9,16,14.4,0.68,47.2 -1612,1359,1,16,16.0,0.72,45.1 -1616,1409,1.1,16,17.6,0.78,44.1 -1620,1444,1.2,16,19.2,0.82,42.5 -1624,1480,1.3,16,20.8,0.87,41.9 -1628,1516,1.45,16,23.2,0.91,39.1 -1632,1549,1.5,16,24.0,0.95,39.5 -1636,1583,1.6,16,25.6,0.99,38.8 -1640,1629,1.8,16,28.8,1.03,35.9 -1644,1668,2,16,32.0,1.10,34.3 -1648,1699,2.1,16,33.6,1.14,34.0 -1652,1740,2.2,16,35.2,1.18,33.6 -1656,1766,2.4,16,38.4,1.24,32.2 -1660,1802,2.6,16,41.6,1.28,30.7 -1664,1835,2.8,16,44.8,1.33,29.7 -1668,1865,2.9,16,46.4,1.39,29.9 -1672,1901,3.1,16,49.6,1.44,29.0 -1676,1930,3.3,16,52.8,1.48,28.1 -1680,1961,3.5,16,56.0,1.54,27.5 -1684,1994,3.7,16,59.2,1.59,26.8 -1688,2031,3.9,16,62.4,1.65,26.5 -1692,2071,4.1,16,65.6,1.69,25.8 -1696,2106,4.3,16,68.8,1.76,25.6 -1700,2129,4.58,16,73.3,1.82,24.9 -1704,2160,4.7,16,75.2,1.88,25.0 -1708,2190,5,16,80.0,1.93,24.2 -1712,2219,5.2,16,83.2,1.99,23.9 -1716,2244,5.42,16,86.7,2.05,23.6 -1720,2266,5.69,16,91.0,2.12,23.3 -1724,2305,5.9,16,94.4,2.18,23.1 -1728,2342,6.19,16,99.0,2.22,22.4 -1732,2392,6.4,16,102.4,2.28,22.2 -1736,2416,6.7,16,107.2,2.38,22.2 -1740,2446,7,16,112.0,2.43,21.7 -1744,2475,7.4,16,118.4,2.52,21.3 -1748,2498,7.7,16,123.2,2.58,20.9 -1752,2538,8.04,16,128.6,2.65,20.6 -1756,2584,8.4,16,134.4,2.73,20.3 -1760,2609,8.7,16,139.2,2.76,19.8 -1764,2639,9.03,16,144.5,2.84,19.7 -1768,2663,9.34,16,149.4,2.89,19.3 -1772,2688,9.69,16,155.0,2.98,19.2 -1776,2711,10.01,16,160.2,3.05,19.1 -1780,2735,10.4,16,166.4,3.11,18.7 -1784,2766,10.68,16,170.9,3.17,18.6 -1788,2809,10.9,16,174.4,3.22,18.4 -1792,2822,11.3,16,180.8,3.30,18.3 -1796,2854,11.64,16,186.2,3.37,18.1 -1800,2888,11.93,16,190.9,3.42,17.9 -1804,2903,12.38,16,198.1,3.51,17.7 -1808,2934,12.79,16,204.6,3.61,17.6 -1812,2973,13.14,16,210.2,3.68,17.5 -1816,3002,13.54,16,216.6,3.74,17.3 -1820,3042,13.99,16,223.8,3.82,17.1 -1824,3065,14.46,16,231.4,3.89,16.8 -1828,3108,14.86,16,237.8,3.96,16.6 -1832,3125,15.4,16,246.4,4.06,16.5 -1836,3153,15.86,16,253.8,4.15,16.3 -1840,3180,16.41,16,262.6,4.25,16.2 -1844,3204,16.97,16,271.5,4.30,15.9 -1848,3242,17.42,16,278.7,4.38,15.7 -1852,3260,18,16,288.0,4.51,15.7 -1856,3299,18.5,16,296.0,4.53,15.3 -1860,3308,19.13,16,306.1,4.65,15.2 -1864,3351,19.59,16,313.4,4.71,15.0 -1868,3379,20.15,16,322.4,4.79,14.8 -1872,3406,20.67,16,330.7,4.84,14.6 -1876,3428,21.13,16,338.1,4.93,14.6 -1880,3438,21.8,16,348.8,5.01,14.4 -1884,3455,22.38,16,358.1,5.08,14.2 -1888,3494,22.81,16,365.0,5.14,14.1 -1892,3516,23.28,16,372.5,5.18,13.9 -1896,3527,23.9,16,382.4,5.22,13.7 -1900,3533,23.83,16,381.3,5.25,13.8 + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) +1100,3465,24.30,16,388.8,-4.07,10.5 +1104,3468,24.3,16,388.8,-4.05,10.4 +1108,3449,23.78,16,380.5,-4.02,10.6 +1112,3421,23.25,16,372.0,-3.96,10.7 +1116,3410,22.64,16,362.2,-3.90,10.8 +1120,3395,22.07,16,353.1,-3.87,11.0 +1124,3374,21.46,16,343.4,-3.82,11.1 +1128,3356,20.98,16,335.7,-3.80,11.3 +1132,3323,20.44,16,327.0,-3.75,11.5 +1136,3307,19.9,16,318.4,-3.71,11.7 +1140,3282,19.32,16,309.1,-3.66,11.8 +1144,3248,18.74,16,299.8,-3.59,12.0 +1148,3223,18.11,16,289.8,-3.52,12.1 +1152,3207,17.53,16,280.5,-3.45,12.3 +1156,3177,17,16,272.0,-3.40,12.5 +1160,3153,16.45,16,263.2,-3.31,12.6 +1164,3111,15.96,16,255.4,-3.25,12.7 +1168,3086,15.48,16,247.7,-3.20,12.9 +1172,3061,14.98,16,239.7,-3.11,13.0 +1176,3050,14.43,16,230.9,-3.04,13.2 +1180,3003,14.04,16,224.6,-2.99,13.3 +1184,2988,13.55,16,216.8,-2.94,13.6 +1188,2952,13.1,16,209.6,-2.86,13.7 +1192,2918,12.74,16,203.8,-2.82,13.8 +1196,2896,12.3,16,196.8,-2.77,14.1 +1200,2865,11.93,16,190.9,-2.71,14.2 +1204,2845,11.59,16,185.4,-2.66,14.3 +1208,2821,11.2,16,179.2,-2.58,14.4 +1212,2792,10.8,16,172.8,-2.55,14.8 +1216,2759,10.5,16,168.0,-2.51,14.9 +1220,2736,10.27,16,164.3,-2.45,14.9 +1224,2712,9.9,16,158.4,-2.38,15.0 +1228,2681,9.5,16,152.0,-2.35,15.5 +1232,2661,9.2,16,147.2,-2.28,15.5 +1236,2629,8.9,16,142.4,-2.24,15.7 +1240,2599,8.6,16,137.6,-2.20,16.0 +1244,2576,8.2,16,131.2,-2.12,16.1 +1248,2548,7.9,16,126.4,-2.08,16.5 +1252,2517,7.57,16,121.1,-2.02,16.7 +1256,2482,7.19,16,115.0,-1.98,17.2 +1260,2440,6.89,16,110.2,-1.94,17.6 +1264,2419,6.53,16,104.5,-1.86,17.8 +1268,2369,6.29,16,100.6,-1.81,18.0 +1272,2345,6,16,96.0,-1.76,18.4 +1276,2306,5.75,16,92.0,-1.70,18.5 +1280,2278,5.5,16,88.0,-1.67,19.0 +1284,2248,5.3,16,84.8,-1.61,19.0 +1288,2222,5,16,80.0,-1.56,19.4 +1292,2199,4.8,16,76.8,-1.51,19.7 +1296,2157,4.6,16,73.6,-1.49,20.2 +1300,2140,4.4,16,70.4,-1.44,20.4 +1304,2106,4.2,16,67.2,-1.40,20.8 +1308,2081,3.98,16,63.7,-1.35,21.2 +1312,2040,3.7,16,59.2,-1.30,22.0 +1316,2001,3.58,16,57.3,-1.26,21.9 +1320,1968,3.39,16,54.2,-1.20,22.2 +1324,1936,3.2,16,51.2,-1.16,22.7 +1328,1903,3,16,48.0,-1.12,23.3 +1332,1870,2.84,16,45.4,-1.10,24.3 +1336,1840,2.7,16,43.2,-1.05,24.4 +1340,1807,2.4,16,38.4,-1.02,26.6 +1344,1773,2.3,16,36.8,-0.98,26.6 +1348,1738,2.1,16,33.6,-0.94,28.1 +1352,1705,2,16,32.0,-0.90,28.2 +1356,1667,1.9,16,30.4,-0.87,28.6 +1360,1630,1.7,16,27.2,-0.82,30.2 +1364,1589,1.6,16,25.6,-0.78,30.7 +1368,1554,1.5,16,24.0,-0.74,31.0 +1372,1519,1.4,16,22.4,-0.72,32.0 +1376,1482,1.3,16,20.8,-0.68,32.7 +1380,1448,1.2,16,19.2,-0.65,33.8 +1384,1410,1.1,16,17.6,-0.62,35.1 +1388,1362,1,16,16.0,-0.58,36.3 +1392,1325,0.9,16,14.4,-0.54,37.5 +1396,1285,0.8,16,12.8,-0.51,40.0 +1400,1246,0.7,16,11.2,-0.48,42.9 +1404,1199,0.66,16,10.6,-0.44,42.1 +1408,1153,0.56,16,9.0,-0.42,46.6 +1412,1113,0.5,16,8.0,-0.39,48.2 +1416,1064,0.4,16,6.4,-0.35,55.3 +1420,1016,0.4,16,6.4,-0.32,50.3 +1424,970,0.37,16,5.9,-0.29,49.8 +1428,916,0.3,16,4.8,-0.26,53.9 +1432,869,0.2,16,3.2,-0.24,73.7 +1436,810,0.2,16,3.2,-0.21,65.2 +1440,758,0.2,16,3.2,-0.18,56.7 +1444,697,0.1,16,1.6,-0.15,93.6 +1448,641,0.1,16,1.6,-0.13,79.4 +1452,578,0.1,16,1.6,-0.10,65.2 +1456,513,0.05,16,0.8,-0.09,107.7 +1460,450,0.05,16,0.8,-0.07,85.0 +1464,388,0.05,16,0.8,-0.05,62.4 +1468,317,0.05,16,0.8,-0.04,45.4 +1472,0,0,16,0.0,0.00,0.0 +1476,0,0,16,0.0,0.00,0.0 +1480,0,0,16,0.0,0.00,0.0 +1484,0,0,16,0.0,0.00,0.0 +1488,0,0,16,0.0,0.00,0.0 +1492,0,0,16,0.0,0.00,0.0 +1496,0,0,16,0.0,0.00,0.0 +1500,0,0,16,0.0,0.00,0.0 +1504,0,0,16,0.0,0.00,0.0 +1508,0,0,16,0.0,0.00,0.0 +1512,0,0,16,0.0,0.00,0.0 +1516,0,0,16,0.0,0.00,0.0 +1520,0,0,16,0.0,0.00,0.0 +1524,0,0,16,0.0,0.00,0.0 +1528,0,0,16,0.0,0.00,0.0 +1532,308,0.05,16,0.8,0.04,51.0 +1536,375,0.05,16,0.8,0.05,68.0 +1540,441,0.05,16,0.8,0.08,96.4 +1544,508,0.05,16,0.8,0.10,124.7 +1548,573,0.1,16,1.6,0.13,79.4 +1552,630,0.1,16,1.6,0.15,96.4 +1556,688,0.1,16,1.6,0.18,113.4 +1560,749,0.2,16,3.2,0.22,68.0 +1564,804,0.2,16,3.2,0.25,78.0 +1568,859,0.2,16,3.2,0.29,90.7 +1572,906,0.3,16,4.8,0.32,67.1 +1576,962,0.4,16,6.4,0.36,56.0 +1580,1007,0.4,16,6.4,0.40,62.4 +1584,1057,0.5,16,8.0,0.44,54.4 +1588,1104,0.5,16,8.0,0.47,59.0 +1592,1148,0.6,16,9.6,0.51,53.4 +1596,1195,0.7,16,11.2,0.56,49.8 +1600,1237,0.8,16,12.8,0.60,47.1 +1604,1279,0.8,16,12.8,0.64,50.0 +1608,1319,0.9,16,14.4,0.68,47.2 +1612,1359,1,16,16.0,0.72,45.1 +1616,1409,1.1,16,17.6,0.78,44.1 +1620,1444,1.2,16,19.2,0.82,42.5 +1624,1480,1.3,16,20.8,0.87,41.9 +1628,1516,1.45,16,23.2,0.91,39.1 +1632,1549,1.5,16,24.0,0.95,39.5 +1636,1583,1.6,16,25.6,0.99,38.8 +1640,1629,1.8,16,28.8,1.03,35.9 +1644,1668,2,16,32.0,1.10,34.3 +1648,1699,2.1,16,33.6,1.14,34.0 +1652,1740,2.2,16,35.2,1.18,33.6 +1656,1766,2.4,16,38.4,1.24,32.2 +1660,1802,2.6,16,41.6,1.28,30.7 +1664,1835,2.8,16,44.8,1.33,29.7 +1668,1865,2.9,16,46.4,1.39,29.9 +1672,1901,3.1,16,49.6,1.44,29.0 +1676,1930,3.3,16,52.8,1.48,28.1 +1680,1961,3.5,16,56.0,1.54,27.5 +1684,1994,3.7,16,59.2,1.59,26.8 +1688,2031,3.9,16,62.4,1.65,26.5 +1692,2071,4.1,16,65.6,1.69,25.8 +1696,2106,4.3,16,68.8,1.76,25.6 +1700,2129,4.58,16,73.3,1.82,24.9 +1704,2160,4.7,16,75.2,1.88,25.0 +1708,2190,5,16,80.0,1.93,24.2 +1712,2219,5.2,16,83.2,1.99,23.9 +1716,2244,5.42,16,86.7,2.05,23.6 +1720,2266,5.69,16,91.0,2.12,23.3 +1724,2305,5.9,16,94.4,2.18,23.1 +1728,2342,6.19,16,99.0,2.22,22.4 +1732,2392,6.4,16,102.4,2.28,22.2 +1736,2416,6.7,16,107.2,2.38,22.2 +1740,2446,7,16,112.0,2.43,21.7 +1744,2475,7.4,16,118.4,2.52,21.3 +1748,2498,7.7,16,123.2,2.58,20.9 +1752,2538,8.04,16,128.6,2.65,20.6 +1756,2584,8.4,16,134.4,2.73,20.3 +1760,2609,8.7,16,139.2,2.76,19.8 +1764,2639,9.03,16,144.5,2.84,19.7 +1768,2663,9.34,16,149.4,2.89,19.3 +1772,2688,9.69,16,155.0,2.98,19.2 +1776,2711,10.01,16,160.2,3.05,19.1 +1780,2735,10.4,16,166.4,3.11,18.7 +1784,2766,10.68,16,170.9,3.17,18.6 +1788,2809,10.9,16,174.4,3.22,18.4 +1792,2822,11.3,16,180.8,3.30,18.3 +1796,2854,11.64,16,186.2,3.37,18.1 +1800,2888,11.93,16,190.9,3.42,17.9 +1804,2903,12.38,16,198.1,3.51,17.7 +1808,2934,12.79,16,204.6,3.61,17.6 +1812,2973,13.14,16,210.2,3.68,17.5 +1816,3002,13.54,16,216.6,3.74,17.3 +1820,3042,13.99,16,223.8,3.82,17.1 +1824,3065,14.46,16,231.4,3.89,16.8 +1828,3108,14.86,16,237.8,3.96,16.6 +1832,3125,15.4,16,246.4,4.06,16.5 +1836,3153,15.86,16,253.8,4.15,16.3 +1840,3180,16.41,16,262.6,4.25,16.2 +1844,3204,16.97,16,271.5,4.30,15.9 +1848,3242,17.42,16,278.7,4.38,15.7 +1852,3260,18,16,288.0,4.51,15.7 +1856,3299,18.5,16,296.0,4.53,15.3 +1860,3308,19.13,16,306.1,4.65,15.2 +1864,3351,19.59,16,313.4,4.71,15.0 +1868,3379,20.15,16,322.4,4.79,14.8 +1872,3406,20.67,16,330.7,4.84,14.6 +1876,3428,21.13,16,338.1,4.93,14.6 +1880,3438,21.8,16,348.8,5.01,14.4 +1884,3455,22.38,16,358.1,5.08,14.2 +1888,3494,22.81,16,365.0,5.14,14.1 +1892,3516,23.28,16,372.5,5.18,13.9 +1896,3527,23.9,16,382.4,5.22,13.7 +1900,3533,23.83,16,381.3,5.25,13.8 diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-18V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-18V.csv index 31efb4baa..ee5517736 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-18V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-18V.csv @@ -1,202 +1,202 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) -1100,3643,28.22,18,507.9,-4.59,9.0 -1104,3643,27.91,18,502.4,-4.54,9.0 -1108,3624,27.31,18,491.6,-4.53,9.2 -1112,3607,26.62,18,479.2,-4.48,9.3 -1116,3578,26.04,18,468.7,-4.42,9.4 -1120,3563,25.38,18,456.8,-4.35,9.5 -1124,3528,24.8,18,446.4,-4.35,9.7 -1128,3520,24.03,18,432.5,-4.17,9.6 -1132,3483,23.53,18,423.5,-4.18,9.9 -1136,3470,22.78,18,410.0,-4.11,10.0 -1140,3441,22.21,18,399.8,-4.04,10.1 -1144,3422,21.5,18,387.0,-3.99,10.3 -1148,3420,20.61,18,371.0,-3.91,10.6 -1152,3375,20.17,18,363.1,-3.86,10.6 -1156,3343,19.51,18,351.2,-3.81,10.8 -1160,3316,18.85,18,339.3,-3.71,10.9 -1164,3273,18.23,18,328.1,-3.68,11.2 -1168,3248,17.57,18,316.3,-3.56,11.2 -1172,3195,17.12,18,308.2,-3.54,11.5 -1176,3193,16.34,18,294.1,-3.42,11.6 -1180,3153,15.8,18,284.4,-3.36,11.8 -1184,3127,15.19,18,273.4,-3.27,11.9 -1188,3087,14.69,18,264.4,-3.19,12.1 -1192,3057,14.16,18,254.9,-3.09,12.1 -1196,3022,13.68,18,246.2,-3.03,12.3 -1200,2985,13.27,18,238.9,-2.96,12.4 -1204,2958,12.83,18,230.9,-2.88,12.5 -1208,2925,12.36,18,222.5,-2.84,12.8 -1212,2896,11.99,18,215.8,-2.78,12.9 -1216,2866,11.58,18,208.4,-2.70,12.9 -1220,2829,11.32,18,203.8,-2.66,13.0 -1224,2806,11,18,198.0,-2.59,13.1 -1228,2785,10.65,18,191.7,-2.53,13.2 -1232,2756,10.27,18,184.9,-2.48,13.4 -1236,2720,9.97,18,179.5,-2.42,13.5 -1240,2695,9.55,18,171.9,-2.36,13.7 -1244,2673,9.2,18,165.6,-2.32,14.0 -1248,2638,8.87,18,159.7,-2.25,14.1 -1252,2603,8.54,18,153.7,-2.21,14.4 -1256,2572,8.19,18,147.4,-2.16,14.6 -1260,2549,7.77,18,139.9,-2.11,15.1 -1264,2514,7.39,18,133.0,-2.04,15.3 -1268,2488,7,18,126.0,-2.00,15.8 -1272,2444,6.65,18,119.7,-1.94,16.2 -1276,2411,6.3,18,113.4,-1.88,16.6 -1280,2379,6,18,108.0,-1.81,16.8 -1284,2343,5.75,18,103.5,-1.76,17.0 -1288,2310,5.5,18,99.0,-1.72,17.4 -1292,2283,5.23,18,94.1,-1.66,17.6 -1296,2247,5,18,90.0,-1.61,17.9 -1300,2218,4.79,18,86.2,-1.56,18.1 -1304,2178,4.59,18,82.6,-1.52,18.4 -1308,2155,4.3,18,77.4,-1.47,18.9 -1312,2123,4.1,18,73.8,-1.42,19.3 -1316,2089,3.9,18,70.2,-1.38,19.6 -1320,2046,3.68,18,66.2,-1.33,20.1 -1324,2013,3.4,18,61.2,-1.27,20.8 -1328,1974,3.3,18,59.4,-1.22,20.6 -1332,1937,3.1,18,55.8,-1.17,21.1 -1336,1896,2.9,18,52.2,-1.13,21.7 -1340,1871,2.73,18,49.1,-1.08,22.1 -1344,1832,2.5,18,45.0,-1.05,23.3 -1348,1796,2.3,18,41.4,-1.01,24.4 -1352,1765,2.14,18,38.5,-0.97,25.2 -1356,1733,2,18,36.0,-0.93,26.0 -1360,1697,1.9,18,34.2,-0.89,26.1 -1364,1659,1.76,18,31.7,-0.86,27.1 -1368,1611,1.6,18,28.8,-0.81,28.2 -1372,1575,1.5,18,27.0,-0.77,28.6 -1376,1539,1.4,18,25.2,-0.74,29.3 -1380,1507,1.3,18,23.4,-0.70,29.9 -1384,1469,1.2,18,21.6,-0.67,31.1 -1388,1428,1.1,18,19.8,-0.64,32.1 -1392,1390,1,18,18.0,-0.60,33.3 -1396,1344,0.8,18,14.4,-0.56,38.7 -1400,1308,0.8,18,14.4,-0.53,36.9 -1404,1270,0.7,18,12.6,-0.49,39.2 -1408,1229,0.63,18,11.3,-0.46,40.8 -1412,1178,0.53,18,9.5,-0.43,45.2 -1416,1136,0.5,18,9.0,-0.40,44.4 -1420,1090,0.4,18,7.2,-0.37,51.0 -1424,1039,0.4,18,7.2,-0.34,46.6 -1428,989,0.3,18,5.4,-0.30,56.3 -1432,935,0.3,18,5.4,-0.27,50.4 -1436,884,0.2,18,3.6,-0.24,66.8 -1440,823,0.2,18,3.6,-0.21,58.0 -1444,760,0.2,18,3.6,-0.18,50.4 -1448,703,0.1,18,1.8,-0.15,85.7 -1452,638,0.1,18,1.8,-0.13,70.6 -1456,575,0.1,18,1.8,-0.10,58.0 -1460,501,0.05,18,0.9,-0.08,85.7 -1464,428,0.05,18,0.9,-0.06,65.5 -1468,357,0.05,18,0.9,-0.05,50.4 -1472,0,0,18,0.0,0.00,0.0 -1476,0,0,18,0.0,0.00,0.0 -1480,0,0,18,0.0,0.00,0.0 -1484,0,0,18,0.0,0.00,0.0 -1488,0,0,18,0.0,0.00,0.0 -1492,0,0,18,0.0,0.00,0.0 -1496,0,0,18,0.0,0.00,0.0 -1500,0,0,18,0.0,0.00,0.0 -1504,0,0,18,0.0,0.00,0.0 -1508,0,0,18,0.0,0.00,0.0 -1512,0,0,18,0.0,0.00,0.0 -1516,0,0,18,0.0,0.00,0.0 -1520,0,0,18,0.0,0.00,0.0 -1524,0,0,18,0.0,0.00,0.0 -1528,0,0,18,0.0,0.00,0.0 -1532,347,0.05,18,0.9,0.05,50.4 -1536,424,0.05,18,0.9,0.07,75.6 -1540,494,0.05,18,0.9,0.09,100.8 -1544,562,0.1,18,1.8,0.12,68.0 -1548,624,0.1,18,1.8,0.15,83.2 -1552,686,0.1,18,1.8,0.18,100.8 -1556,745,0.2,18,3.6,0.21,59.2 -1560,805,0.2,18,3.6,0.25,69.3 -1564,856,0.2,18,3.6,0.29,79.4 -1568,906,0.3,18,5.4,0.32,58.8 -1572,959,0.3,18,5.4,0.36,66.4 -1576,1009,0.4,18,7.2,0.39,54.2 -1580,1052,0.4,18,7.2,0.43,59.2 -1584,1099,0.5,18,9.0,0.47,52.4 -1588,1140,0.5,18,9.0,0.51,56.4 -1592,1184,0.6,18,10.8,0.55,50.8 -1596,1227,0.7,18,12.6,0.59,46.4 -1600,1267,0.8,18,14.4,0.63,43.5 -1604,1301,0.88,18,15.8,0.67,42.1 -1608,1341,1,18,18.0,0.70,38.8 -1612,1377,1,18,18.0,0.76,42.1 -1616,1419,1.2,18,21.6,0.80,37.2 -1620,1456,1.3,18,23.4,0.84,35.9 -1624,1493,1.38,18,24.8,0.88,35.4 -1628,1527,1.5,18,27.0,0.92,33.9 -1632,1564,1.6,18,28.8,0.97,33.5 -1636,1599,1.7,18,30.6,1.02,33.2 -1640,1637,1.83,18,32.9,1.08,32.6 -1644,1679,2,18,36.0,1.13,31.5 -1648,1716,2.1,18,37.8,1.18,31.2 -1652,1747,2.3,18,41.4,1.22,29.6 -1656,1783,2.47,18,44.5,1.28,28.8 -1660,1817,2.7,18,48.6,1.32,27.2 -1664,1851,2.9,18,52.2,1.38,26.5 -1668,1887,3.07,18,55.3,1.42,25.8 -1672,1920,3.24,18,58.3,1.49,25.5 -1676,1952,3.4,18,61.2,1.54,25.1 -1680,1989,3.6,18,64.8,1.60,24.6 -1684,2021,3.8,18,68.4,1.68,24.5 -1688,2066,4.07,18,73.3,1.74,23.8 -1692,2098,4.29,18,77.2,1.79,23.1 -1696,2143,4.52,18,81.4,1.84,22.6 -1700,2171,4.72,18,85.0,1.92,22.6 -1704,2198,4.98,18,89.6,1.97,22.0 -1708,2230,5.2,18,93.6,2.04,21.8 -1712,2265,5.47,18,98.5,2.08,21.1 -1716,2293,5.7,18,102.6,2.17,21.2 -1720,2330,5.97,18,107.5,2.22,20.6 -1724,2351,6.25,18,112.5,2.28,20.3 -1728,2402,6.55,18,117.9,2.36,20.0 -1732,2443,6.87,18,123.7,2.43,19.7 -1736,2474,7.28,18,131.0,2.50,19.1 -1740,2513,7.64,18,137.5,2.59,18.8 -1744,2546,8,18,144.0,2.64,18.4 -1748,2577,8.4,18,151.2,2.74,18.1 -1752,2602,8.8,18,158.4,2.79,17.6 -1756,2627,9.15,18,164.7,2.87,17.4 -1760,2670,9.48,18,170.6,2.93,17.1 -1764,2695,9.9,18,178.2,3.01,16.9 -1768,2731,10.2,18,183.6,3.10,16.9 -1772,2766,10.53,18,189.5,3.15,16.6 -1776,2809,10.81,18,194.6,3.21,16.5 -1780,2823,11.18,18,201.2,3.27,16.2 -1784,2850,11.58,18,208.4,3.39,16.3 -1788,2894,11.88,18,213.8,3.46,16.2 -1792,2914,12.29,18,221.2,3.54,16.0 -1796,2960,12.59,18,226.6,3.63,16.0 -1800,2985,13.12,18,236.2,3.72,15.8 -1804,3026,13.5,18,243.0,3.79,15.6 -1808,3055,14.04,18,252.7,3.92,15.5 -1812,3089,14.49,18,260.8,4.02,15.4 -1816,3124,15.09,18,271.6,4.14,15.2 -1820,3180,15.55,18,279.9,4.21,15.0 -1824,3222,16.14,18,290.5,4.32,14.9 -1828,3265,16.66,18,299.9,4.41,14.7 -1832,3283,17.32,18,311.8,4.54,14.5 -1836,3324,17.89,18,322.0,4.67,14.5 -1840,3375,18.45,18,332.1,4.69,14.1 -1844,3391,19.27,18,346.9,4.86,14.0 -1848,3417,19.72,18,355.0,4.92,13.9 -1852,3446,20.38,18,366.8,5.02,13.7 -1856,3482,21.03,18,378.5,5.12,13.5 -1860,3513,21.6,18,388.8,5.21,13.4 -1864,3542,22.29,18,401.2,5.29,13.2 -1868,3572,22.87,18,411.7,5.38,13.1 -1872,3597,23.55,18,423.9,5.50,13.0 -1876,3619,24.23,18,436.1,5.60,12.8 -1880,3637,24.77,18,445.9,5.67,12.7 -1884,3653,25.48,18,458.6,5.77,12.6 -1888,3687,26.04,18,468.7,5.82,12.4 -1892,3711,26.74,18,481.3,5.90,12.3 -1896,3751,27.22,18,490.0,5.98,12.2 -1900,3742,27.57,18,496.3,6.02,12.1 + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) +1100,3643,28.22,18,507.9,-4.59,9.0 +1104,3643,27.91,18,502.4,-4.54,9.0 +1108,3624,27.31,18,491.6,-4.53,9.2 +1112,3607,26.62,18,479.2,-4.48,9.3 +1116,3578,26.04,18,468.7,-4.42,9.4 +1120,3563,25.38,18,456.8,-4.35,9.5 +1124,3528,24.8,18,446.4,-4.35,9.7 +1128,3520,24.03,18,432.5,-4.17,9.6 +1132,3483,23.53,18,423.5,-4.18,9.9 +1136,3470,22.78,18,410.0,-4.11,10.0 +1140,3441,22.21,18,399.8,-4.04,10.1 +1144,3422,21.5,18,387.0,-3.99,10.3 +1148,3420,20.61,18,371.0,-3.91,10.6 +1152,3375,20.17,18,363.1,-3.86,10.6 +1156,3343,19.51,18,351.2,-3.81,10.8 +1160,3316,18.85,18,339.3,-3.71,10.9 +1164,3273,18.23,18,328.1,-3.68,11.2 +1168,3248,17.57,18,316.3,-3.56,11.2 +1172,3195,17.12,18,308.2,-3.54,11.5 +1176,3193,16.34,18,294.1,-3.42,11.6 +1180,3153,15.8,18,284.4,-3.36,11.8 +1184,3127,15.19,18,273.4,-3.27,11.9 +1188,3087,14.69,18,264.4,-3.19,12.1 +1192,3057,14.16,18,254.9,-3.09,12.1 +1196,3022,13.68,18,246.2,-3.03,12.3 +1200,2985,13.27,18,238.9,-2.96,12.4 +1204,2958,12.83,18,230.9,-2.88,12.5 +1208,2925,12.36,18,222.5,-2.84,12.8 +1212,2896,11.99,18,215.8,-2.78,12.9 +1216,2866,11.58,18,208.4,-2.70,12.9 +1220,2829,11.32,18,203.8,-2.66,13.0 +1224,2806,11,18,198.0,-2.59,13.1 +1228,2785,10.65,18,191.7,-2.53,13.2 +1232,2756,10.27,18,184.9,-2.48,13.4 +1236,2720,9.97,18,179.5,-2.42,13.5 +1240,2695,9.55,18,171.9,-2.36,13.7 +1244,2673,9.2,18,165.6,-2.32,14.0 +1248,2638,8.87,18,159.7,-2.25,14.1 +1252,2603,8.54,18,153.7,-2.21,14.4 +1256,2572,8.19,18,147.4,-2.16,14.6 +1260,2549,7.77,18,139.9,-2.11,15.1 +1264,2514,7.39,18,133.0,-2.04,15.3 +1268,2488,7,18,126.0,-2.00,15.8 +1272,2444,6.65,18,119.7,-1.94,16.2 +1276,2411,6.3,18,113.4,-1.88,16.6 +1280,2379,6,18,108.0,-1.81,16.8 +1284,2343,5.75,18,103.5,-1.76,17.0 +1288,2310,5.5,18,99.0,-1.72,17.4 +1292,2283,5.23,18,94.1,-1.66,17.6 +1296,2247,5,18,90.0,-1.61,17.9 +1300,2218,4.79,18,86.2,-1.56,18.1 +1304,2178,4.59,18,82.6,-1.52,18.4 +1308,2155,4.3,18,77.4,-1.47,18.9 +1312,2123,4.1,18,73.8,-1.42,19.3 +1316,2089,3.9,18,70.2,-1.38,19.6 +1320,2046,3.68,18,66.2,-1.33,20.1 +1324,2013,3.4,18,61.2,-1.27,20.8 +1328,1974,3.3,18,59.4,-1.22,20.6 +1332,1937,3.1,18,55.8,-1.17,21.1 +1336,1896,2.9,18,52.2,-1.13,21.7 +1340,1871,2.73,18,49.1,-1.08,22.1 +1344,1832,2.5,18,45.0,-1.05,23.3 +1348,1796,2.3,18,41.4,-1.01,24.4 +1352,1765,2.14,18,38.5,-0.97,25.2 +1356,1733,2,18,36.0,-0.93,26.0 +1360,1697,1.9,18,34.2,-0.89,26.1 +1364,1659,1.76,18,31.7,-0.86,27.1 +1368,1611,1.6,18,28.8,-0.81,28.2 +1372,1575,1.5,18,27.0,-0.77,28.6 +1376,1539,1.4,18,25.2,-0.74,29.3 +1380,1507,1.3,18,23.4,-0.70,29.9 +1384,1469,1.2,18,21.6,-0.67,31.1 +1388,1428,1.1,18,19.8,-0.64,32.1 +1392,1390,1,18,18.0,-0.60,33.3 +1396,1344,0.8,18,14.4,-0.56,38.7 +1400,1308,0.8,18,14.4,-0.53,36.9 +1404,1270,0.7,18,12.6,-0.49,39.2 +1408,1229,0.63,18,11.3,-0.46,40.8 +1412,1178,0.53,18,9.5,-0.43,45.2 +1416,1136,0.5,18,9.0,-0.40,44.4 +1420,1090,0.4,18,7.2,-0.37,51.0 +1424,1039,0.4,18,7.2,-0.34,46.6 +1428,989,0.3,18,5.4,-0.30,56.3 +1432,935,0.3,18,5.4,-0.27,50.4 +1436,884,0.2,18,3.6,-0.24,66.8 +1440,823,0.2,18,3.6,-0.21,58.0 +1444,760,0.2,18,3.6,-0.18,50.4 +1448,703,0.1,18,1.8,-0.15,85.7 +1452,638,0.1,18,1.8,-0.13,70.6 +1456,575,0.1,18,1.8,-0.10,58.0 +1460,501,0.05,18,0.9,-0.08,85.7 +1464,428,0.05,18,0.9,-0.06,65.5 +1468,357,0.05,18,0.9,-0.05,50.4 +1472,0,0,18,0.0,0.00,0.0 +1476,0,0,18,0.0,0.00,0.0 +1480,0,0,18,0.0,0.00,0.0 +1484,0,0,18,0.0,0.00,0.0 +1488,0,0,18,0.0,0.00,0.0 +1492,0,0,18,0.0,0.00,0.0 +1496,0,0,18,0.0,0.00,0.0 +1500,0,0,18,0.0,0.00,0.0 +1504,0,0,18,0.0,0.00,0.0 +1508,0,0,18,0.0,0.00,0.0 +1512,0,0,18,0.0,0.00,0.0 +1516,0,0,18,0.0,0.00,0.0 +1520,0,0,18,0.0,0.00,0.0 +1524,0,0,18,0.0,0.00,0.0 +1528,0,0,18,0.0,0.00,0.0 +1532,347,0.05,18,0.9,0.05,50.4 +1536,424,0.05,18,0.9,0.07,75.6 +1540,494,0.05,18,0.9,0.09,100.8 +1544,562,0.1,18,1.8,0.12,68.0 +1548,624,0.1,18,1.8,0.15,83.2 +1552,686,0.1,18,1.8,0.18,100.8 +1556,745,0.2,18,3.6,0.21,59.2 +1560,805,0.2,18,3.6,0.25,69.3 +1564,856,0.2,18,3.6,0.29,79.4 +1568,906,0.3,18,5.4,0.32,58.8 +1572,959,0.3,18,5.4,0.36,66.4 +1576,1009,0.4,18,7.2,0.39,54.2 +1580,1052,0.4,18,7.2,0.43,59.2 +1584,1099,0.5,18,9.0,0.47,52.4 +1588,1140,0.5,18,9.0,0.51,56.4 +1592,1184,0.6,18,10.8,0.55,50.8 +1596,1227,0.7,18,12.6,0.59,46.4 +1600,1267,0.8,18,14.4,0.63,43.5 +1604,1301,0.88,18,15.8,0.67,42.1 +1608,1341,1,18,18.0,0.70,38.8 +1612,1377,1,18,18.0,0.76,42.1 +1616,1419,1.2,18,21.6,0.80,37.2 +1620,1456,1.3,18,23.4,0.84,35.9 +1624,1493,1.38,18,24.8,0.88,35.4 +1628,1527,1.5,18,27.0,0.92,33.9 +1632,1564,1.6,18,28.8,0.97,33.5 +1636,1599,1.7,18,30.6,1.02,33.2 +1640,1637,1.83,18,32.9,1.08,32.6 +1644,1679,2,18,36.0,1.13,31.5 +1648,1716,2.1,18,37.8,1.18,31.2 +1652,1747,2.3,18,41.4,1.22,29.6 +1656,1783,2.47,18,44.5,1.28,28.8 +1660,1817,2.7,18,48.6,1.32,27.2 +1664,1851,2.9,18,52.2,1.38,26.5 +1668,1887,3.07,18,55.3,1.42,25.8 +1672,1920,3.24,18,58.3,1.49,25.5 +1676,1952,3.4,18,61.2,1.54,25.1 +1680,1989,3.6,18,64.8,1.60,24.6 +1684,2021,3.8,18,68.4,1.68,24.5 +1688,2066,4.07,18,73.3,1.74,23.8 +1692,2098,4.29,18,77.2,1.79,23.1 +1696,2143,4.52,18,81.4,1.84,22.6 +1700,2171,4.72,18,85.0,1.92,22.6 +1704,2198,4.98,18,89.6,1.97,22.0 +1708,2230,5.2,18,93.6,2.04,21.8 +1712,2265,5.47,18,98.5,2.08,21.1 +1716,2293,5.7,18,102.6,2.17,21.2 +1720,2330,5.97,18,107.5,2.22,20.6 +1724,2351,6.25,18,112.5,2.28,20.3 +1728,2402,6.55,18,117.9,2.36,20.0 +1732,2443,6.87,18,123.7,2.43,19.7 +1736,2474,7.28,18,131.0,2.50,19.1 +1740,2513,7.64,18,137.5,2.59,18.8 +1744,2546,8,18,144.0,2.64,18.4 +1748,2577,8.4,18,151.2,2.74,18.1 +1752,2602,8.8,18,158.4,2.79,17.6 +1756,2627,9.15,18,164.7,2.87,17.4 +1760,2670,9.48,18,170.6,2.93,17.1 +1764,2695,9.9,18,178.2,3.01,16.9 +1768,2731,10.2,18,183.6,3.10,16.9 +1772,2766,10.53,18,189.5,3.15,16.6 +1776,2809,10.81,18,194.6,3.21,16.5 +1780,2823,11.18,18,201.2,3.27,16.2 +1784,2850,11.58,18,208.4,3.39,16.3 +1788,2894,11.88,18,213.8,3.46,16.2 +1792,2914,12.29,18,221.2,3.54,16.0 +1796,2960,12.59,18,226.6,3.63,16.0 +1800,2985,13.12,18,236.2,3.72,15.8 +1804,3026,13.5,18,243.0,3.79,15.6 +1808,3055,14.04,18,252.7,3.92,15.5 +1812,3089,14.49,18,260.8,4.02,15.4 +1816,3124,15.09,18,271.6,4.14,15.2 +1820,3180,15.55,18,279.9,4.21,15.0 +1824,3222,16.14,18,290.5,4.32,14.9 +1828,3265,16.66,18,299.9,4.41,14.7 +1832,3283,17.32,18,311.8,4.54,14.5 +1836,3324,17.89,18,322.0,4.67,14.5 +1840,3375,18.45,18,332.1,4.69,14.1 +1844,3391,19.27,18,346.9,4.86,14.0 +1848,3417,19.72,18,355.0,4.92,13.9 +1852,3446,20.38,18,366.8,5.02,13.7 +1856,3482,21.03,18,378.5,5.12,13.5 +1860,3513,21.6,18,388.8,5.21,13.4 +1864,3542,22.29,18,401.2,5.29,13.2 +1868,3572,22.87,18,411.7,5.38,13.1 +1872,3597,23.55,18,423.9,5.50,13.0 +1876,3619,24.23,18,436.1,5.60,12.8 +1880,3637,24.77,18,445.9,5.67,12.7 +1884,3653,25.48,18,458.6,5.77,12.6 +1888,3687,26.04,18,468.7,5.82,12.4 +1892,3711,26.74,18,481.3,5.90,12.3 +1896,3751,27.22,18,490.0,5.98,12.2 +1900,3742,27.57,18,496.3,6.02,12.1 diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-20V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-20V.csv index 165881815..cc3112f14 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-20V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-20V.csv @@ -1,202 +1,202 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W),, -1100,3783,32.17,20,643.5,-5.04,7.8,, -1104,3777,31.37,20,627.4,-4.96,7.9,, -1108,3740,30.6,20,612.0,-4.92,8.0,, -1112,3741,29.67,20,593.4,-4.84,8.2,, -1116,3712,29.04,20,580.8,-4.79,8.3,, -1120,3681,28.26,20,565.2,-4.74,8.4,, -1124,3639,27.56,20,551.2,-4.68,8.5,, -1128,3642,26.74,20,534.8,-4.58,8.6,, -1132,3624,25.82,20,516.4,-4.46,8.6,, -1136,3569,25.16,20,503.2,-4.40,8.8,, -1140,3558,24.45,20,489.0,-4.35,8.9,, -1144,3571,23.54,20,470.8,-4.23,9.0,, -1148,3498,23.08,20,461.6,-4.16,9.0,, -1152,3449,22.46,20,449.2,-4.12,9.2,, -1156,3457,21.63,20,432.6,-4.02,9.3,, -1160,3390,21,20,420.0,-3.96,9.4,, -1164,3385,20.3,20,406.0,-3.89,9.6,, -1168,3335,19.65,20,393.0,-3.81,9.7,, -1172,3315,19,20,380.0,-3.71,9.8,, -1176,3293,18.36,20,367.2,-3.66,10.0,, -1180,3250,17.75,20,355.0,-3.56,10.0,, -1184,3211,17.11,20,342.2,-3.48,10.2,, -1188,3184,16.43,20,328.6,-3.36,10.2,, -1192,3139,15.81,20,316.2,-3.29,10.4,, -1196,3107,15.23,20,304.6,-3.16,10.4,, -1200,3046,14.74,20,294.8,-3.13,10.6,, -1204,3034,14.12,20,282.4,-3.00,10.6,, -1208,2983,13.66,20,273.2,-2.94,10.8,, -1212,2952,13.22,20,264.4,-2.87,10.9,, -1216,2927,12.69,20,253.8,-2.78,10.9,, -1220,2891,12.36,20,247.2,-2.74,11.1,, -1224,2856,12.01,20,240.2,-2.67,11.1,, -1228,2824,11.57,20,231.4,-2.60,11.3,, -1232,2797,11.27,20,225.4,-2.54,11.3,, -1236,2766,10.84,20,216.8,-2.50,11.5,, -1240,2731,10.54,20,210.8,-2.44,11.6,, -1244,2698,10.22,20,204.4,-2.40,11.8,, -1248,2679,9.77,20,195.4,-2.31,11.8,, -1252,2636,9.4,20,188.0,-2.26,12.0,, -1256,2623,8.96,20,179.2,-2.20,12.3,, -1260,2579,8.6,20,172.0,-2.14,12.4,, -1264,2554,8.29,20,165.8,-2.11,12.7,, -1268,2523,7.87,20,157.4,-2.04,13.0,, -1272,2487,7.48,20,149.6,-1.97,13.2,, -1276,2453,7.03,20,140.6,-1.91,13.6,, -1280,2414,6.6,20,132.0,-1.86,14.1,, -1284,2367,6.3,20,126.0,-1.80,14.3,, -1288,2346,5.97,20,119.4,-1.73,14.5,, -1292,2300,5.7,20,114.0,-1.67,14.7,, -1296,2265,5.41,20,108.2,-1.64,15.1,, -1300,2229,5.16,20,103.2,-1.58,15.3,, -1304,2192,4.88,20,97.6,-1.52,15.6,, -1308,2166,4.6,20,92.0,-1.47,16.0,, -1312,2135,4.31,20,86.2,-1.41,16.4,, -1316,2093,4.09,20,81.8,-1.37,16.8,, -1320,2050,3.81,20,76.2,-1.32,17.4,, -1324,2009,3.58,20,71.6,-1.27,17.7,, -1328,1972,3.32,20,66.4,-1.22,18.4,, -1332,1927,3.09,20,61.8,-1.17,18.9,, -1336,1895,2.9,20,58.0,-1.11,19.1,, -1340,1853,2.71,20,54.2,-1.07,19.7,, -1344,1816,2.46,20,49.2,-1.02,20.7,, -1348,1779,2.3,20,46.0,-0.99,21.5,, -1352,1748,2.1,20,42.0,-0.94,22.5,, -1356,1718,2,20,40.0,-0.91,22.7,, -1360,1676,1.84,20,36.8,-0.88,23.8,, -1364,1643,1.7,20,34.0,-0.83,24.4,, -1368,1597,1.59,20,31.8,-0.78,24.5,, -1372,1560,1.5,20,30.0,-0.75,25.1,, -1376,1526,1.3,20,26.0,-0.72,27.6,, -1380,1493,1.29,20,25.8,-0.69,26.7,, -1384,1461,1.2,20,24.0,-0.66,27.4,, -1388,1426,1.05,20,21.0,-0.63,30.0,, -1392,1388,1,20,20.0,-0.59,29.5,, -1396,1349,0.88,20,17.6,-0.56,31.7,, -1400,1315,0.8,20,16.0,-0.53,33.2,, -1404,1277,0.7,20,14.0,-0.50,36.0,, -1408,1246,0.7,20,14.0,-0.47,33.7,, -1412,1201,0.6,20,12.0,-0.44,37.0,, -1416,1159,0.5,20,10.0,-0.41,41.3,, -1420,1120,0.5,20,10.0,-0.39,38.6,, -1424,1077,0.4,20,8.0,-0.36,44.8,, -1428,1024,0.4,20,8.0,-0.32,40.3,, -1432,982,0.3,20,6.0,-0.29,49.1,, -1436,925,0.2,20,4.0,-0.26,65.8,, -1440,876,0.2,20,4.0,-0.24,59.0,, -1444,813,0.2,20,4.0,-0.20,51.0,, -1448,752,0.1,20,2.0,-0.17,86.2,, -1452,688,0.1,20,2.0,-0.15,72.6,, -1456,624,0.1,20,2.0,-0.12,61.2,, -1460,547,0.05,20,1.0,-0.09,90.7,, -1464,471,0.05,20,1.0,-0.07,72.6,, -1468,389,0.05,20,1.0,-0.05,54.4,, -1472,310,0.05,20,1.0,-0.04,36.3,, -1476,0,0,20,0.0,0.00,0.0,, -1480,0,0,20,0.0,0.00,0.0,, -1484,0,0,20,0.0,0.00,0.0,, -1488,0,0,20,0.0,0.00,0.0,, -1492,0,0,20,0.0,0.00,0.0,, -1496,0,0,20,0.0,0.00,0.0,, -1500,0,0,20,0.0,0.00,0.0,, -1504,0,0,20,0.0,0.00,0.0,, -1508,0,0,20,0.0,0.00,0.0,, -1512,0,0,20,0.0,0.00,0.0,, -1516,0,0,20,0.0,0.00,0.0,, -1520,0,0,20,0.0,0.00,0.0,, -1524,0,0,20,0.0,0.00,0.0,, -1528,0,0,20,0.0,0.00,0.0,, -1532,379,0.05,20,1.0,0.06,59.0,, -1536,457,0.05,20,1.0,0.09,86.2,, -1540,532,0.05,20,1.0,0.11,113.4,, -1544,605,0.1,20,2.0,0.15,74.8,, -1548,676,0.1,20,2.0,0.18,90.7,, -1552,735,0.1,20,2.0,0.21,106.6,, -1556,797,0.2,20,4.0,0.25,62.4,, -1560,857,0.2,20,4.0,0.29,71.4,, -1564,905,0.2,20,4.0,0.32,80.5,, -1568,963,0.3,20,6.0,0.36,60.5,, -1572,1001,0.4,20,8.0,0.39,48.8,, -1576,1046,0.4,20,8.0,0.43,53.3,, -1580,1092,0.5,20,10.0,0.47,46.7,, -1584,1129,0.5,20,10.0,0.50,49.9,, -1588,1162,0.6,20,12.0,0.54,44.6,, -1592,1207,0.7,20,14.0,0.57,40.8,, -1596,1240,0.7,20,14.0,0.61,43.4,, -1600,1273,0.8,20,16.0,0.64,40.0,, -1604,1305,0.8,20,16.0,0.66,41.4,, -1608,1334,0.96,20,19.2,0.70,36.4,, -1612,1367,1,20,20.0,0.74,37.0,, -1616,1407,1.1,20,22.0,0.78,35.5,, -1620,1441,1.2,20,24.0,0.82,34.0,, -1624,1472,1.3,20,26.0,0.86,33.1,, -1628,1506,1.4,20,28.0,0.89,31.8,, -1632,1537,1.51,20,30.2,0.93,30.9,, -1636,1571,1.6,20,32.0,0.98,30.5,, -1640,1603,1.79,20,35.8,1.02,28.5,, -1644,1644,1.9,20,38.0,1.06,27.9,, -1648,1685,2.06,20,41.2,1.12,27.2,, -1652,1720,2.2,20,44.0,1.17,26.6,, -1656,1751,2.37,20,47.4,1.22,25.7,, -1660,1797,2.63,20,52.6,1.28,24.3,, -1664,1832,2.82,20,56.4,1.31,23.2,, -1668,1868,3.06,20,61.2,1.38,22.5,, -1672,1911,3.28,20,65.6,1.44,21.9,, -1676,1950,3.49,20,69.8,1.53,21.9,, -1680,1991,3.72,20,74.4,1.58,21.3,, -1684,2040,3.98,20,79.6,1.64,20.6,, -1688,2072,4.26,20,85.2,1.74,20.4,, -1692,2110,4.59,20,91.8,1.80,19.6,, -1696,2145,4.83,20,96.6,1.85,19.1,, -1700,2186,5.08,20,101.6,1.92,18.9,, -1704,2218,5.39,20,107.8,1.98,18.4,, -1708,2242,5.66,20,113.2,2.07,18.3,, -1712,2292,5.9,20,118.0,2.12,18.0,, -1716,2321,6.2,20,124.0,2.20,17.7,, -1720,2371,6.5,20,130.0,2.25,17.3,, -1724,2419,6.87,20,137.4,2.33,16.9,, -1728,2452,7.3,20,146.0,2.44,16.7,, -1732,2493,7.72,20,154.4,2.52,16.3,, -1736,2520,8.14,20,162.8,2.61,16.0,, -1740,2552,8.53,20,170.6,2.66,15.6,, -1744,2593,8.92,20,178.4,2.76,15.5,, -1748,2621,9.29,20,185.8,2.83,15.2,, -1752,2654,9.7,20,194.0,2.91,15.0,, -1756,2696,10,20,200.0,2.99,15.0,, -1760,2723,10.43,20,208.6,3.06,14.7,, -1764,2752,10.77,20,215.4,3.13,14.6,, -1768,2788,11.12,20,222.4,3.22,14.5,, -1772,2820,11.47,20,229.4,3.28,14.3,, -1776,2861,11.78,20,235.6,3.36,14.2,, -1780,2887,12.23,20,244.6,3.44,14.1,, -1784,2928,12.62,20,252.4,3.55,14.1,, -1788,2968,13,20,260.0,3.64,14.0,, -1792,3013,13.47,20,269.4,3.72,13.8,, -1796,3039,14,20,280.0,3.85,13.7,, -1800,3088,14.46,20,289.2,3.96,13.7,, -1804,3122,15.07,20,301.4,4.10,13.6,, -1808,3182,15.64,20,312.8,4.21,13.5,, -1812,3221,16.27,20,325.4,4.33,13.3,, -1816,3268,16.83,20,336.6,4.42,13.1,, -1820,3304,17.51,20,350.2,4.55,13.0,, -1824,3341,18.07,20,361.4,4.70,13.0,, -1828,3380,18.69,20,373.8,4.82,12.9,, -1832,3418,19.3,20,386.0,4.93,12.8,, -1836,3453,19.98,20,399.6,5.02,12.6,, -1840,3481,20.63,20,412.6,5.14,12.5,, -1844,3519,21.21,20,424.2,5.20,12.3,, -1848,3561,21.88,20,437.6,5.39,12.3,, -1852,3593,22.49,20,449.8,5.52,12.3,, -1856,3614,23.2,20,464.0,5.62,12.1,, -1860,3660,23.8,20,476.0,5.71,12.0,, -1864,3684,24.55,20,491.0,5.82,11.8,, -1868,3721,25.19,20,503.8,5.93,11.8,, -1872,3732,25.95,20,519.0,6.06,11.7,, -1876,3778,26.62,20,532.4,6.13,11.5,, -1880,3801,27.55,20,551.0,6.26,11.4,, -1884,3837,28.12,20,562.4,6.36,11.3,, -1888,3855,28.95,20,579.0,6.46,11.2,, -1892,3877,29.74,20,594.8,6.55,11.0,, -1896,3907,30.46,20,609.2,6.62,10.9,, -1900,3931,31.21,20,624.2,6.72,10.8,, + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W),, +1100,3783,32.17,20,643.5,-5.04,7.8,, +1104,3777,31.37,20,627.4,-4.96,7.9,, +1108,3740,30.6,20,612.0,-4.92,8.0,, +1112,3741,29.67,20,593.4,-4.84,8.2,, +1116,3712,29.04,20,580.8,-4.79,8.3,, +1120,3681,28.26,20,565.2,-4.74,8.4,, +1124,3639,27.56,20,551.2,-4.68,8.5,, +1128,3642,26.74,20,534.8,-4.58,8.6,, +1132,3624,25.82,20,516.4,-4.46,8.6,, +1136,3569,25.16,20,503.2,-4.40,8.8,, +1140,3558,24.45,20,489.0,-4.35,8.9,, +1144,3571,23.54,20,470.8,-4.23,9.0,, +1148,3498,23.08,20,461.6,-4.16,9.0,, +1152,3449,22.46,20,449.2,-4.12,9.2,, +1156,3457,21.63,20,432.6,-4.02,9.3,, +1160,3390,21,20,420.0,-3.96,9.4,, +1164,3385,20.3,20,406.0,-3.89,9.6,, +1168,3335,19.65,20,393.0,-3.81,9.7,, +1172,3315,19,20,380.0,-3.71,9.8,, +1176,3293,18.36,20,367.2,-3.66,10.0,, +1180,3250,17.75,20,355.0,-3.56,10.0,, +1184,3211,17.11,20,342.2,-3.48,10.2,, +1188,3184,16.43,20,328.6,-3.36,10.2,, +1192,3139,15.81,20,316.2,-3.29,10.4,, +1196,3107,15.23,20,304.6,-3.16,10.4,, +1200,3046,14.74,20,294.8,-3.13,10.6,, +1204,3034,14.12,20,282.4,-3.00,10.6,, +1208,2983,13.66,20,273.2,-2.94,10.8,, +1212,2952,13.22,20,264.4,-2.87,10.9,, +1216,2927,12.69,20,253.8,-2.78,10.9,, +1220,2891,12.36,20,247.2,-2.74,11.1,, +1224,2856,12.01,20,240.2,-2.67,11.1,, +1228,2824,11.57,20,231.4,-2.60,11.3,, +1232,2797,11.27,20,225.4,-2.54,11.3,, +1236,2766,10.84,20,216.8,-2.50,11.5,, +1240,2731,10.54,20,210.8,-2.44,11.6,, +1244,2698,10.22,20,204.4,-2.40,11.8,, +1248,2679,9.77,20,195.4,-2.31,11.8,, +1252,2636,9.4,20,188.0,-2.26,12.0,, +1256,2623,8.96,20,179.2,-2.20,12.3,, +1260,2579,8.6,20,172.0,-2.14,12.4,, +1264,2554,8.29,20,165.8,-2.11,12.7,, +1268,2523,7.87,20,157.4,-2.04,13.0,, +1272,2487,7.48,20,149.6,-1.97,13.2,, +1276,2453,7.03,20,140.6,-1.91,13.6,, +1280,2414,6.6,20,132.0,-1.86,14.1,, +1284,2367,6.3,20,126.0,-1.80,14.3,, +1288,2346,5.97,20,119.4,-1.73,14.5,, +1292,2300,5.7,20,114.0,-1.67,14.7,, +1296,2265,5.41,20,108.2,-1.64,15.1,, +1300,2229,5.16,20,103.2,-1.58,15.3,, +1304,2192,4.88,20,97.6,-1.52,15.6,, +1308,2166,4.6,20,92.0,-1.47,16.0,, +1312,2135,4.31,20,86.2,-1.41,16.4,, +1316,2093,4.09,20,81.8,-1.37,16.8,, +1320,2050,3.81,20,76.2,-1.32,17.4,, +1324,2009,3.58,20,71.6,-1.27,17.7,, +1328,1972,3.32,20,66.4,-1.22,18.4,, +1332,1927,3.09,20,61.8,-1.17,18.9,, +1336,1895,2.9,20,58.0,-1.11,19.1,, +1340,1853,2.71,20,54.2,-1.07,19.7,, +1344,1816,2.46,20,49.2,-1.02,20.7,, +1348,1779,2.3,20,46.0,-0.99,21.5,, +1352,1748,2.1,20,42.0,-0.94,22.5,, +1356,1718,2,20,40.0,-0.91,22.7,, +1360,1676,1.84,20,36.8,-0.88,23.8,, +1364,1643,1.7,20,34.0,-0.83,24.4,, +1368,1597,1.59,20,31.8,-0.78,24.5,, +1372,1560,1.5,20,30.0,-0.75,25.1,, +1376,1526,1.3,20,26.0,-0.72,27.6,, +1380,1493,1.29,20,25.8,-0.69,26.7,, +1384,1461,1.2,20,24.0,-0.66,27.4,, +1388,1426,1.05,20,21.0,-0.63,30.0,, +1392,1388,1,20,20.0,-0.59,29.5,, +1396,1349,0.88,20,17.6,-0.56,31.7,, +1400,1315,0.8,20,16.0,-0.53,33.2,, +1404,1277,0.7,20,14.0,-0.50,36.0,, +1408,1246,0.7,20,14.0,-0.47,33.7,, +1412,1201,0.6,20,12.0,-0.44,37.0,, +1416,1159,0.5,20,10.0,-0.41,41.3,, +1420,1120,0.5,20,10.0,-0.39,38.6,, +1424,1077,0.4,20,8.0,-0.36,44.8,, +1428,1024,0.4,20,8.0,-0.32,40.3,, +1432,982,0.3,20,6.0,-0.29,49.1,, +1436,925,0.2,20,4.0,-0.26,65.8,, +1440,876,0.2,20,4.0,-0.24,59.0,, +1444,813,0.2,20,4.0,-0.20,51.0,, +1448,752,0.1,20,2.0,-0.17,86.2,, +1452,688,0.1,20,2.0,-0.15,72.6,, +1456,624,0.1,20,2.0,-0.12,61.2,, +1460,547,0.05,20,1.0,-0.09,90.7,, +1464,471,0.05,20,1.0,-0.07,72.6,, +1468,389,0.05,20,1.0,-0.05,54.4,, +1472,310,0.05,20,1.0,-0.04,36.3,, +1476,0,0,20,0.0,0.00,0.0,, +1480,0,0,20,0.0,0.00,0.0,, +1484,0,0,20,0.0,0.00,0.0,, +1488,0,0,20,0.0,0.00,0.0,, +1492,0,0,20,0.0,0.00,0.0,, +1496,0,0,20,0.0,0.00,0.0,, +1500,0,0,20,0.0,0.00,0.0,, +1504,0,0,20,0.0,0.00,0.0,, +1508,0,0,20,0.0,0.00,0.0,, +1512,0,0,20,0.0,0.00,0.0,, +1516,0,0,20,0.0,0.00,0.0,, +1520,0,0,20,0.0,0.00,0.0,, +1524,0,0,20,0.0,0.00,0.0,, +1528,0,0,20,0.0,0.00,0.0,, +1532,379,0.05,20,1.0,0.06,59.0,, +1536,457,0.05,20,1.0,0.09,86.2,, +1540,532,0.05,20,1.0,0.11,113.4,, +1544,605,0.1,20,2.0,0.15,74.8,, +1548,676,0.1,20,2.0,0.18,90.7,, +1552,735,0.1,20,2.0,0.21,106.6,, +1556,797,0.2,20,4.0,0.25,62.4,, +1560,857,0.2,20,4.0,0.29,71.4,, +1564,905,0.2,20,4.0,0.32,80.5,, +1568,963,0.3,20,6.0,0.36,60.5,, +1572,1001,0.4,20,8.0,0.39,48.8,, +1576,1046,0.4,20,8.0,0.43,53.3,, +1580,1092,0.5,20,10.0,0.47,46.7,, +1584,1129,0.5,20,10.0,0.50,49.9,, +1588,1162,0.6,20,12.0,0.54,44.6,, +1592,1207,0.7,20,14.0,0.57,40.8,, +1596,1240,0.7,20,14.0,0.61,43.4,, +1600,1273,0.8,20,16.0,0.64,40.0,, +1604,1305,0.8,20,16.0,0.66,41.4,, +1608,1334,0.96,20,19.2,0.70,36.4,, +1612,1367,1,20,20.0,0.74,37.0,, +1616,1407,1.1,20,22.0,0.78,35.5,, +1620,1441,1.2,20,24.0,0.82,34.0,, +1624,1472,1.3,20,26.0,0.86,33.1,, +1628,1506,1.4,20,28.0,0.89,31.8,, +1632,1537,1.51,20,30.2,0.93,30.9,, +1636,1571,1.6,20,32.0,0.98,30.5,, +1640,1603,1.79,20,35.8,1.02,28.5,, +1644,1644,1.9,20,38.0,1.06,27.9,, +1648,1685,2.06,20,41.2,1.12,27.2,, +1652,1720,2.2,20,44.0,1.17,26.6,, +1656,1751,2.37,20,47.4,1.22,25.7,, +1660,1797,2.63,20,52.6,1.28,24.3,, +1664,1832,2.82,20,56.4,1.31,23.2,, +1668,1868,3.06,20,61.2,1.38,22.5,, +1672,1911,3.28,20,65.6,1.44,21.9,, +1676,1950,3.49,20,69.8,1.53,21.9,, +1680,1991,3.72,20,74.4,1.58,21.3,, +1684,2040,3.98,20,79.6,1.64,20.6,, +1688,2072,4.26,20,85.2,1.74,20.4,, +1692,2110,4.59,20,91.8,1.80,19.6,, +1696,2145,4.83,20,96.6,1.85,19.1,, +1700,2186,5.08,20,101.6,1.92,18.9,, +1704,2218,5.39,20,107.8,1.98,18.4,, +1708,2242,5.66,20,113.2,2.07,18.3,, +1712,2292,5.9,20,118.0,2.12,18.0,, +1716,2321,6.2,20,124.0,2.20,17.7,, +1720,2371,6.5,20,130.0,2.25,17.3,, +1724,2419,6.87,20,137.4,2.33,16.9,, +1728,2452,7.3,20,146.0,2.44,16.7,, +1732,2493,7.72,20,154.4,2.52,16.3,, +1736,2520,8.14,20,162.8,2.61,16.0,, +1740,2552,8.53,20,170.6,2.66,15.6,, +1744,2593,8.92,20,178.4,2.76,15.5,, +1748,2621,9.29,20,185.8,2.83,15.2,, +1752,2654,9.7,20,194.0,2.91,15.0,, +1756,2696,10,20,200.0,2.99,15.0,, +1760,2723,10.43,20,208.6,3.06,14.7,, +1764,2752,10.77,20,215.4,3.13,14.6,, +1768,2788,11.12,20,222.4,3.22,14.5,, +1772,2820,11.47,20,229.4,3.28,14.3,, +1776,2861,11.78,20,235.6,3.36,14.2,, +1780,2887,12.23,20,244.6,3.44,14.1,, +1784,2928,12.62,20,252.4,3.55,14.1,, +1788,2968,13,20,260.0,3.64,14.0,, +1792,3013,13.47,20,269.4,3.72,13.8,, +1796,3039,14,20,280.0,3.85,13.7,, +1800,3088,14.46,20,289.2,3.96,13.7,, +1804,3122,15.07,20,301.4,4.10,13.6,, +1808,3182,15.64,20,312.8,4.21,13.5,, +1812,3221,16.27,20,325.4,4.33,13.3,, +1816,3268,16.83,20,336.6,4.42,13.1,, +1820,3304,17.51,20,350.2,4.55,13.0,, +1824,3341,18.07,20,361.4,4.70,13.0,, +1828,3380,18.69,20,373.8,4.82,12.9,, +1832,3418,19.3,20,386.0,4.93,12.8,, +1836,3453,19.98,20,399.6,5.02,12.6,, +1840,3481,20.63,20,412.6,5.14,12.5,, +1844,3519,21.21,20,424.2,5.20,12.3,, +1848,3561,21.88,20,437.6,5.39,12.3,, +1852,3593,22.49,20,449.8,5.52,12.3,, +1856,3614,23.2,20,464.0,5.62,12.1,, +1860,3660,23.8,20,476.0,5.71,12.0,, +1864,3684,24.55,20,491.0,5.82,11.8,, +1868,3721,25.19,20,503.8,5.93,11.8,, +1872,3732,25.95,20,519.0,6.06,11.7,, +1876,3778,26.62,20,532.4,6.13,11.5,, +1880,3801,27.55,20,551.0,6.26,11.4,, +1884,3837,28.12,20,562.4,6.36,11.3,, +1888,3855,28.95,20,579.0,6.46,11.2,, +1892,3877,29.74,20,594.8,6.55,11.0,, +1896,3907,30.46,20,609.2,6.62,10.9,, +1900,3931,31.21,20,624.2,6.72,10.8,, diff --git a/motion/thruster_interface_auv/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..90feb69b9 100644 --- a/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_driver_lib.py +++ b/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_driver_lib.py @@ -1,71 +1,68 @@ #!/usr/bin/env python3 # Import libraries -import smbus2 -import pandas import numpy +import pandas +import smbus2 class ThrusterInterfaceAUVDriver: def __init__( self, - I2C_BUS=1, - PICO_I2C_ADDRESS=0x21, - SYSTEM_OPERATIONAL_VOLTAGE=16.0, - ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET="", - THRUSTER_MAPPING=[7, 6, 5, 4, 3, 2, 1, 0], - THRUSTER_DIRECTION=[1, 1, 1, 1, 1, 1, 1, 1], - THRUSTER_PWM_OFFSET=[0, 0, 0, 0, 0, 0, 0, 0], - PWM_MIN=[1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100], - PWM_MAX=[1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900], - ): - # Initialice the I2C comunication + i2c_bus: int = 1, + pico_i2c_address: int = 0x21, + system_operational_voltage: float = 16.0, + ros2_package_name_for_thruster_datasheet: str = "", + thruster_mapping: list[int] = [7, 6, 5, 4, 3, 2, 1, 0], + thruster_direction: list[int] = [1, 1, 1, 1, 1, 1, 1, 1], + thruster_pwm_offset: list[int] = [0, 0, 0, 0, 0, 0, 0, 0], + pwm_min: list[int] = [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100], + pwm_max: list[int] = [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900], + ) -> None: + # Initialice the I2C communication self.bus = None try: - self.bus = smbus2.SMBus(I2C_BUS) - except Exception as 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 bus nr {self.bus}: {error_code}") + self.pico_i2c_address = pico_i2c_address # Set mapping, direction and offset for the thrusters - self.THRUSTER_MAPPING = THRUSTER_MAPPING - self.THRUSTER_DIRECTION = THRUSTER_DIRECTION - self.THRUSTER_PWM_OFFSET = THRUSTER_PWM_OFFSET - self.PWM_MIN = PWM_MIN - self.PWM_MAX = PWM_MAX + 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 + # This is because we have multiple files for the behaviours of the thrusters depending on the voltage of the drone + if system_operational_voltage < 11.0: + self.system_operational_voltage = 10 + elif system_operational_voltage < 13.0: + self.system_operational_voltage = 12 + elif system_operational_voltage < 15.0: + self.system_operational_voltage = 14 + elif system_operational_voltage < 17.0: + self.system_operational_voltage = 16 + elif system_operational_voltage < 19.0: + self.system_operational_voltage = 18 + elif system_operational_voltage >= 19.0: + self.system_operational_voltage = 20 # Get the full path to the ROS2 package this file is located at - self.ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET = ( - ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET + self.ros2_package_name_for_thruster_datasheet = ( + ros2_package_name_for_thruster_datasheet ) - def _interpolate_forces_to_pwm(self, thruster_forces_array): - """ - Takes in Array of forces in Newtosn [N] - takes 8 floats in form of: - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + def _interpolate_forces_to_pwm(self, thruster_forces_array: list) -> list: + """Takes in Array of forces in Newtosn [N] takes 8 floats in form of: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]. Returns an Array of PWM Gives out 8 ints in form of: [0, 0, 0, 0, 0, 0, 0, 0] """ # Read the important data from the .csv file - 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 +71,16 @@ 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 @@ -87,65 +88,61 @@ 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 # 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]) # 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): - """ - Takes in Array of forces in Newtosn [N] - takes 8 floats in form of: - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + def drive_thrusters(self, thruster_forces_array: list) -> list: + """Takes in Array of forces in Newtosn [N] takes 8 floats in form of: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]. Converts Forces to PWM signals PWM signals sent to PCA9685 module through I2C PCA9685 Module sends electrical PWM signals to the individual thruster ESCs The ESCs send corecponding electrical power to the Thrustres - Thrusters then generate thrust 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: [0, 0, 0, 0, 0, 0, 0, 0] """ - # Apply thruster mapping and direction thruster_forces_array = [ - thruster_forces_array[i] * self.THRUSTER_DIRECTION[i] - for i in self.THRUSTER_MAPPING + thruster_forces_array[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 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] - 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 + # Send data through I2C to the microcontroller that then controls the ESC and extension the thrusters try: self._send_data_to_escs(thruster_pwm_array) - except Exception as errorCode: - print(f"ERROR: Failed to send PWM values: {errorCode}") + 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 7fb44a573..31ba8a5c1 100755 --- a/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py +++ b/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py @@ -1,19 +1,20 @@ #!/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 # Custom libraries from vortex_msgs.msg import ThrusterForces + from thruster_interface_auv.thruster_interface_auv_driver_lib import ( ThrusterInterfaceAUVDriver, ) class ThrusterInterfaceAUVNode(Node): - def __init__(self): + def __init__(self) -> None: # Initialize and name the node process running super().__init__("thruster_interface_auv_node") @@ -52,13 +53,13 @@ def __init__(self): self.thruster_direction = self.get_parameter( "propulsion.thrusters.thruster_direction" ).value - self.thruster_PWM_offset = self.get_parameter( + self.thruster_pwm_offset = self.get_parameter( "propulsion.thrusters.thruster_PWM_offset" ).value - self.thruster_PWM_min = self.get_parameter( + self.thruster_pwm_min = self.get_parameter( "propulsion.thrusters.thruster_PWM_min" ).value - self.thruster_PWM_max = self.get_parameter( + self.thruster_pwm_max = self.get_parameter( "propulsion.thrusters.thruster_PWM_max" ).value self.thrust_timer_period = ( @@ -67,14 +68,14 @@ def __init__(self): # Initialize thruster driver self.thruster_driver = ThrusterInterfaceAUVDriver( - ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET=get_package_share_directory( + ros2_package_name_for_thruster_datasheet=get_package_share_directory( "thruster_interface_auv" ), - THRUSTER_MAPPING=self.thruster_mapping, - THRUSTER_DIRECTION=self.thruster_direction, - THRUSTER_PWM_OFFSET=self.thruster_PWM_offset, - PWM_MIN=self.thruster_PWM_min, - PWM_MAX=self.thruster_PWM_max, + thruster_mapping=self.thruster_mapping, + thruster_direction=self.thruster_direction, + thruster_pwm_offset=self.thruster_pwm_offset, + pwm_min=self.thruster_pwm_min, + pwm_max=self.thruster_pwm_max, ) # Start clock timer for driving thrusters every 0.2 seconds @@ -86,13 +87,13 @@ 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 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 ) @@ -102,7 +103,17 @@ 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. + + 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) diff --git a/perception-auv b/perception-auv deleted file mode 160000 index 6f91a3113..000000000 --- a/perception-auv +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 6f91a3113182bcc3b64d7b6e78eef4e9ea53ca7a diff --git a/ruff.toml b/ruff.toml new file mode 100644 index 000000000..bf3554d64 --- /dev/null +++ b/ruff.toml @@ -0,0 +1,6 @@ +[lint.pydocstyle] +convention = "google" + +[format] +# Keep quotes as is +quote-style = "preserve" diff --git a/scripts/activate_thrusters.sh b/scripts/activate_thrusters.sh index 007b4cbab..503831149 100755 --- a/scripts/activate_thrusters.sh +++ b/scripts/activate_thrusters.sh @@ -2,12 +2,11 @@ echo "##ARMING##"; rostopic pub /thrust/desired_forces geometry_msgs/Wrench "force: - x: 0.0 - y: 0.0 - z: 0.0 -torque: - x: 0.0 - y: 0.0 + x: 0.0 + y: 0.0 + z: 0.0 +torque: + x: 0.0 + y: 0.0 z: 0.0"; echo "ARMING DONE" ; - 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 diff --git a/scripts/fsm.sh b/scripts/fsm.sh index a8d7c8106..def61e886 100755 --- a/scripts/fsm.sh +++ b/scripts/fsm.sh @@ -1,12 +1,12 @@ #!/bin/bash -# The purpose of this file is to automate launching of +# The purpose of this file is to automate launching of # all nodes found in this repository FOR ROBOSUB 2022 source /opt/ros/noetic/setup.bash source /home/ubuntu/vortex_ws/devel/setup.bash echo "##Activate thruster##" -/home/ubuntu/vortex_ws/src/Vortex-AUV/activate_thrusters.sh & +/home/ubuntu/vortex_ws/src/Vortex-AUV/activate_thrusters.sh & sleep 5s echo "##Starting FSM##" roslaunch finite_state_machine robosub_fsm.launch diff --git a/scripts/perception_sim.sh b/scripts/perception_sim.sh index 6163c01dd..f8656db16 100755 --- a/scripts/perception_sim.sh +++ b/scripts/perception_sim.sh @@ -8,7 +8,7 @@ objectPose: frame_id: '' pose: position: {x: 3.0, y: 1.0, z: -2.0} - orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} + orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} isDetected: true estimateConverged: true estimateFucked: false" & @@ -24,7 +24,7 @@ objectPose: frame_id: '' pose: position: {x: 7.5, y: -2.6, z: -2.0} - orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} + orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} isDetected: true estimateConverged: true estimateFucked: false" & @@ -39,7 +39,7 @@ objectPose: frame_id: '' pose: position: {x: 12.0, y: 4.65, z: -2.0} - orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} + orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} isDetected: true estimateConverged: true estimateFucked: false" & @@ -55,7 +55,7 @@ objectPose: frame_id: '' pose: position: {x: 12.0, y: 4.65, z: -1.8} - orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} + orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} isDetected: true estimateConverged: true estimateFucked: false" & @@ -72,7 +72,7 @@ objectPose: frame_id: '' pose: position: {x: 17.0, y: 2.0, z: -2.0} - orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} + orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} isDetected: true estimateConverged: true -estimateFucked: false" & \ No newline at end of file +estimateFucked: false" &