Implemented TPIK and behavior tree (#25)

* started aruco marker detection

* Started implementing visual odometry interface

* dont die on me now momo

* Implemented support to estimate pose of largest tag in frame

* entered transformation matrix hell

* added tf listener and buffer

* Finished initial untested version of marker detector

* started integrating param config file

* Integrated angler_bringup package to launch full system

* Ran precommit and deleted pep257 tests

* Updated devcontainer to support humble and rolling

* Removed debug log

* Added static marker tf broadcaster example

* un-ignored bringup package in docker build

* Resolved pr comments

* Added skeleton package for planning interface

* Added additional repos file

* Started high level planner implementation

* Implemented initial version of pre-planned mission planner

* precommit

* resolved pr comments

* Removed file associations for action and msg files

* Refactored library and added tests

* Resolved final pr comments

* Refactored GStreamer interface into distinct node (#6)

* Refactored to support future localization sources and handlers

* Resolved PR comments

* Integrate support for localization using the Qualisys motion capture system (#8)

* Added qualisys motion capture support

* Added mocap node to launch file

* Added missing configuration to bringup config

* Modified launch description to support running the mocap or camera regardless of whether or not it is used as a source

* Port angler_localization to blue project (#11)

* Ported angler_localization

* Cleaned up dockerfile

* Moved to rolling devcontainer

* Implement Gazebo + ArduSub SITL for UVMS (#15)

* Ported angler_localization

* Cleaned up dockerfile

* Moved to rolling devcontainer

* Implemented initial versions of mux and demux

* Started to integrate blue and alpha launch files

* Continued integrating blue and alpha into system

* Converted bluerov2 sdf to urdf and split up gazebo configs

* Set up initial manipulator mount link

* Continued integrating uvms model

* Started to cleanup dockerfile to fix gz launch bugs

* [wip] resolved ros2_control launch issues - currently breaking down into smaller files

* Resolved launch issues

* More fixes and added gripper inertial properties

* kinda fixed initial position issue

* fixed bouyancy for bluerov2

* Resolved docker environment errors

* Resolved precommit errors

* Fixed precommit

* Resolved PR comments

* Implement Set-Based TPIK and perform initial cleanup (#17)

* started jacobian implementation

* Implemented jacobians for primary tasks

* finished (untested) initial version of jacobian implementation

* Saving DH param stuff before I delete in case of revert

* One more dh save

* Finished initial untested jacobians and started constraint implementation

* go crazy

* Finished jacobian unit tests

* Cleaned up jacobian implementation in favor of tasks

* Went back on task object idea

* started controller

* Save point before I go back to task classes... again

* Improved task interface

* Implemented base tpik

* Implemented hierarchy and modes

* Finished initial implementation of set based tpik

* Wrote hierarchy unit tests and started debugging

* Fixed set tasks

* Fixed axis d bug, resolved solution selection bug, and added nvidia driver support

* Added nominal configuration task

* Fixed devcontainer and most of end effector tracking

* Debugging uvms position jacobian

* WHY WONT THE POSITION JACOBIAN WORK FOR THE LOVE OF GOD

* i believe in a world free from the curse of singularities

* wip: started cleanup

* wip: removed angler msgs

* wip: fixed waypoint planner

* wip: redid the description package and launch file

* More bug fixes from cleanup

* Finished initial cleanup and tpik implementation

* Cleaned up readme

* Readme changes

* More readme stuff

* Fixed precommit and added iron to ci

* Implement behavior tree (#22)

* Started development of a cleaned up dockerfile

* Continued docker setup

* Finished up updated dockerfile

* here we go again

* Removed armv7

* Added requirement for vtk

* Replaced kinpy installation with fork

* Added updated docker compose script

* Moved tf to blue

* Started integration of a bezier curve planner

* Split kinematics into separate package

* Started arming behaviors

* Removed drake stuff and finished arming behaviors

* Created base controller

* Started implementing trajectory controller

* wip: continued joint trajectory controller implementation

* Finished initial version of trajectory interface

* Rebuild image to include blue changes

* Finished joint trajectory controller

* wip: finished initial version of behavior tree

* removed branch from pipeline

* Started debugging tree

* Fixed data gathering behaviors

* Finished debugging behavior tree

* Finished initial version of behavior tree

* Added gripper and cleanup behaviors

* started trajectory replanner

* Cleaned up params

* cleaned up behavior tree

* Resolved pr comments as much as i feel like resolving them

* Fixed bug in frame names

* updated issue templates

.clang-tidy

@@ -15,7 +15,6 @@ Checks: >
   -misc-non-private-member-variables-in-classes,
   -modernize-return-braced-init-list,
   -modernize-use-trailing-return-type,
-  -performance-move-const-arg,
   -readability-braces-around-statements,
   -readability-identifier-length,
   -readability-magic-numbers,
@@ -25,7 +24,8 @@ Checks: >
   -bugprone-narrowing-conversions,
   -bugprone-easily-swappable-parameters,
   -bugprone-implicit-widening-of-multiplication-result,
-  -modernize-avoid-bind
+  -clang-diagnostic-error,
+  -bugprone-exception-escape
 WarningsAsErrors: "*"
 CheckOptions:
   - key: readability-braces-around-statements.ShortStatementLines

.devcontainer.json → .devcontainer/nouveau/devcontainer.json

@@ -1,20 +1,22 @@
 {
-  "dockerFile": ".docker/Dockerfile",
-  "context": ".",
+  "dockerFile": "../../.docker/Dockerfile",
+  "context": "../..",
   "build": {
     "args": {
       "WORKSPACE": "${containerWorkspaceFolder}",
-      "ROS_DISTRO": "humble"
-    }
+      "ROS_DISTRO": "rolling"
+    },
+    "target": "develop"
   },
-  "remoteUser": "dev",
+  "remoteUser": "angler",
   "runArgs": [
     "--network=host",
     "--cap-add=SYS_PTRACE",
     "--security-opt=seccomp:unconfined",
     "--security-opt=apparmor:unconfined",
     "--volume=/dev:/dev",
-    "--privileged"
+    "--privileged",
+    "--volume=/run/user/1000:/run/user/1000"
   ],
   "containerEnv": {
     "DISPLAY": "${localEnv:DISPLAY}",
@@ -30,12 +32,15 @@
           "*.world": "xml",
           "*.xacro": "xml",
           "*.srdf": "xml",
-          "*.rviz": "yaml"
+          "*.rviz": "yaml",
+          "*.config": "xml",
+          "*.sdf": "xml"
         },
         "terminal.integrated.defaultProfile.linux": "bash",
         "files.insertFinalNewline": true,
         "files.trimTrailingWhitespace": true,
         "editor.formatOnSave": true,
+        "editor.tabSize": 2,
         "xml.format.maxLineWidth": 100,
         "json.format.enable": true,
         "python.linting.enabled": true,
@@ -45,14 +50,18 @@
         "python.linting.mypyEnabled": true,
         "python.formatting.provider": "black",
         "autoDocstring.startOnNewLine": false,
-        "autoDocstring.docstringFormat": "google",
+        "autoDocstring.docstringFormat": "google-notypes",
         "isort.args": ["--profile", "black"],
         "isort.check": true,
         "python.autoComplete.extraPaths": [
-          "/opt/ros/humble/lib/python3.10/site-packages/"
+          "/opt/ros/rolling/lib/python3.10/site-packages/",
+          "/opt/ros/rolling/local/lib/python3.10/dist-packages/",
+          "${workspaceFolder}/install/"
         ],
         "python.analysis.extraPaths": [
-          "/opt/ros/humble/lib/python3.10/site-packages/"
+          "/opt/ros/rolling/lib/python3.10/site-packages/",
+          "/opt/ros/rolling/local/lib/python3.10/dist-packages/",
+          "${workspaceFolder}/install/"
         ],
         "C_Cpp.default.intelliSenseMode": "linux-gcc-x86",
         "C_Cpp.clang_format_fallbackStyle": "Google",
@@ -84,9 +93,6 @@
         "[xml]": {
           "editor.defaultFormatter": "redhat.vscode-xml"
         },
-        "[yaml]": {
-          "editor.defaultFormatter": "redhat.vscode-yaml"
-        },
         "[markdown]": {
           "editor.rulers": [80],
           "editor.defaultFormatter": "DavidAnson.vscode-markdownlint"
@@ -111,7 +117,8 @@
         "esbenp.prettier-vscode",
         "xaver.clang-format",
         "ms-python.isort",
-        "ms-python.flake8"
+        "ms-python.flake8",
+        "ms-python.black-formatter"
       ]
     }
   }

.devcontainer/nvidia/devcontainer.json

@@ -0,0 +1,127 @@
+{
+  "dockerFile": "../../.docker/Dockerfile",
+  "context": "../..",
+  "build": {
+    "args": {
+      "WORKSPACE": "${containerWorkspaceFolder}",
+      "ROS_DISTRO": "rolling"
+    },
+    "target": "develop-nvidia"
+  },
+  "remoteUser": "angler",
+  "runArgs": [
+    "--network=host",
+    "--cap-add=SYS_PTRACE",
+    "--security-opt=seccomp:unconfined",
+    "--security-opt=apparmor:unconfined",
+    "--volume=/tmp/.X11-unix:/tmp/.X11-unix",
+    "--volume=/mnt/wslg:/mnt/wslg",
+    "--gpus=all"
+  ],
+  "containerEnv": {
+    "DISPLAY": "${localEnv:DISPLAY}",
+    "WAYLAND_DISPLAY": "${localEnv:WAYLAND_DISPLAY}",
+    "XDG_RUNTIME_DIR": "${localEnv:XDG_RUNTIME_DIR}",
+    "PULSE_SERVER": "${localEnv:PULSE_SERVER}",
+    "LIBGL_ALWAYS_SOFTWARE": "1",
+    "QT_X11_NO_MITSHM": "1"
+  },
+  "customizations": {
+    "vscode": {
+      "settings": {
+        "files.associations": {
+          "*.repos": "yaml",
+          "*.world": "xml",
+          "*.xacro": "xml",
+          "*.srdf": "xml",
+          "*.rviz": "yaml",
+          "*.config": "xml",
+          "*.sdf": "xml"
+        },
+        "terminal.integrated.defaultProfile.linux": "bash",
+        "files.insertFinalNewline": true,
+        "files.trimTrailingWhitespace": true,
+        "editor.formatOnSave": true,
+        "editor.tabSize": 2,
+        "xml.format.maxLineWidth": 100,
+        "json.format.enable": true,
+        "python.linting.enabled": true,
+        "python.linting.flake8Enabled": true,
+        "python.linting.pylintEnabled": false,
+        "python.linting.pydocstyleEnabled": true,
+        "python.linting.mypyEnabled": true,
+        "python.formatting.provider": "black",
+        "autoDocstring.startOnNewLine": false,
+        "autoDocstring.docstringFormat": "google-notypes",
+        "isort.args": ["--profile", "black"],
+        "isort.check": true,
+        "python.autoComplete.extraPaths": [
+          "/opt/ros/rolling/lib/python3.10/site-packages/",
+          "/opt/ros/rolling/local/lib/python3.10/dist-packages/",
+          "${workspaceFolder}/install/"
+        ],
+        "python.analysis.extraPaths": [
+          "/opt/ros/rolling/lib/python3.10/site-packages/",
+          "/opt/ros/rolling/local/lib/python3.10/dist-packages/",
+          "${workspaceFolder}/install/"
+        ],
+        "C_Cpp.default.intelliSenseMode": "linux-gcc-x86",
+        "C_Cpp.clang_format_fallbackStyle": "Google",
+        "C_Cpp.codeAnalysis.clangTidy.enabled": true,
+        "C_Cpp.codeAnalysis.clangTidy.runAutomatically": true,
+        "clang-format.executable": "/usr/bin/clang-format-14",
+        "[cpp]": {
+          "editor.rulers": [100],
+          "editor.tabSize": 2,
+          "editor.defaultFormatter": "xaver.clang-format"
+        },
+        "[python]": {
+          "editor.tabSize": 4,
+          "editor.rulers": [90],
+          "editor.codeActionsOnSave": {
+            "source.organizeImports": true
+          }
+        },
+        "[dockerfile]": {
+          "editor.quickSuggestions": {
+            "strings": true
+          },
+          "editor.defaultFormatter": "ms-azuretools.vscode-docker",
+          "editor.tabSize": 4
+        },
+        "[json]": {
+          "editor.defaultFormatter": "esbenp.prettier-vscode"
+        },
+        "[xml]": {
+          "editor.defaultFormatter": "redhat.vscode-xml"
+        },
+        "[markdown]": {
+          "editor.rulers": [80],
+          "editor.defaultFormatter": "DavidAnson.vscode-markdownlint"
+        },
+        "search.exclude": {
+          "**/build": true,
+          "**/install": true,
+          "**/log": true
+        }
+      },
+      "extensions": [
+        "ms-azuretools.vscode-docker",
+        "ms-python.python",
+        "njpwerner.autodocstring",
+        "cschlosser.doxdocgen",
+        "ms-vscode.cpptools",
+        "redhat.vscode-xml",
+        "redhat.vscode-yaml",
+        "josetr.cmake-language-support-vscode",
+        "smilerobotics.urdf",
+        "DavidAnson.vscode-markdownlint",
+        "esbenp.prettier-vscode",
+        "xaver.clang-format",
+        "ms-python.isort",
+        "ms-python.flake8",
+        "ms-python.black-formatter"
+      ]
+    }
+  }
+}