diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
index eea392a..7c2dec8 100644
--- a/.github/CODEOWNERS
+++ b/.github/CODEOWNERS
@@ -5,7 +5,10 @@ Dockerfile @B0gos
 
 src/guppy @cole-wilson
 src/guppy_control @cole-wilson
+src/guppy_description @cole-wilson
+
 src/guppy_can @andy-sorge
 src/guppy_teleop @andy-sorge
-src/guppy_state @ryden-handsaker
-src/guppy_description @cole-wilson
\ No newline at end of file
+src/guppy_vision @andy-sorge
+
+src/guppy_state @ryden-handsaker
\ No newline at end of file
diff --git a/.github/guppy.png b/.github/guppy.png
index 8f310a4..268e73d 100644
Binary files a/.github/guppy.png and b/.github/guppy.png differ
diff --git a/.idea/.gitignore b/.idea/.gitignore
deleted file mode 100644
index ab1f416..0000000
--- a/.idea/.gitignore
+++ /dev/null
@@ -1,10 +0,0 @@
-# Default ignored files
-/shelf/
-/workspace.xml
-# Ignored default folder with query files
-/queries/
-# Datasource local storage ignored files
-/dataSources/
-/dataSources.local.xml
-# Editor-based HTTP Client requests
-/httpRequests/
diff --git a/.idea/codeStyles/Project.xml b/.idea/codeStyles/Project.xml
index 81469bd..4c195d2 100644
--- a/.idea/codeStyles/Project.xml
+++ b/.idea/codeStyles/Project.xml
@@ -1,7 +1,6 @@
 <component name="ProjectCodeStyleConfiguration">
   <code_scheme name="Project" version="173">
     <RiderCodeStyleSettings>
-      <option name="/Default/CodeStyle/CodeFormatting/CppClangFormat/EnableClangFormatSupport/@EntryValue" value="true" type="bool" />
       <option name="/Default/CodeStyle/EditorConfig/EnableClangFormatSupport/@EntryValue" value="false" type="bool" />
       <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_BINARY_EXPRESSIONS_CHAIN/@EntryValue" value="true" type="bool" />
       <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_CALLS_CHAIN/@EntryValue" value="false" type="bool" />
@@ -31,8 +30,6 @@
       <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_CAST_EXPRESSION_PARENTHESES/@EntryValue" value="true" type="bool" />
       <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_AFTER_BINARY_OPSIGN/@EntryValue" value="true" type="bool" />
       <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_BEFORE_TERNARY_OPSIGNS/@EntryValue" value="true" type="bool" />
-      <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/TYPE_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
-      <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/OTHER_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
       <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/CASE_BLOCK_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
       <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/BLANK_LINES_AROUND_FUNCTION_DECLARATION/@EntryValue" value="1" type="int" />
       <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/BLANK_LINES_AROUND_FUNCTION_DEFINITION/@EntryValue" value="1" type="int" />
@@ -89,14 +86,18 @@
       <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_ARGUMENTS_STYLE/@EntryValue" value="WRAP_IF_LONG" type="string" />
       <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_PARAMETERS_STYLE/@EntryValue" value="WRAP_IF_LONG" type="string" />
       <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/BREAK_TEMPLATE_DECLARATION/@EntryValue" value="LINE_BREAK" type="string" />
-      <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/NAMESPACE_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
-      <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INVOCABLE_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
-      <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ANONYMOUS_METHOD_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
       <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INITIALIZER_BRACES/@EntryValue" value="END_OF_LINE_NO_SPACE" type="string" />
-      <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_STYLE/@EntryValue" value="Space" type="string" />
       <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_SIZE/@EntryValue" value="4" type="int" />
       <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/CONTINUOUS_LINE_INDENT/@EntryValue" value="Double" type="string" />
       <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/TAB_WIDTH/@EntryValue" value="4" type="int" />
+      <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/NAMESPACE_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
+      <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/EXPORT_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
+      <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/TYPE_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
+      <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INVOCABLE_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
+      <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ANONYMOUS_METHOD_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
+      <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/REQUIRES_EXPRESSION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
+      <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/OTHER_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
+      <option name="/Default/CodeStyle/CodeFormatting/CppClangFormat/EnableClangFormatSupport/@EntryValue" value="true" type="bool" />
     </RiderCodeStyleSettings>
     <clangFormatSettings>
       <option name="ENABLED" value="true" />
diff --git a/.idea/dictionaries/project.xml b/.idea/dictionaries/project.xml
deleted file mode 100644
index 5c8acb5..0000000
--- a/.idea/dictionaries/project.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-<component name="ProjectDictionaryState">
-  <dictionary name="project">
-    <words>
-      <w>nbytes</w>
-    </words>
-  </dictionary>
-</component>
\ No newline at end of file
diff --git a/.idea/editor.xml b/.idea/editor.xml
index e7b96ca..e46151b 100644
--- a/.idea/editor.xml
+++ b/.idea/editor.xml
@@ -1,250 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
   <component name="BackendCodeEditorSettings">
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CDeclarationWithImplicitIntType/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CommentTypo/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=ConstevalIfIsAlwaysConstant/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppAbstractClassWithoutSpecifier/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppAbstractFinalClass/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppAbstractVirtualFunctionCallInCtor/@EntryIndexedValue" value="ERROR" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppAccessSpecifierWithNoDeclarations/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppAwaiterTypeIsNotClass/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppBooleanIncrementExpression/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppBoostFormatBadCode/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppBoostFormatLegacyCode/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppBoostFormatMixedArgs/@EntryIndexedValue" value="ERROR" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppBoostFormatTooFewArgs/@EntryIndexedValue" value="ERROR" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppBoostFormatTooManyArgs/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppCStyleCast/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppCVQualifierCanNotBeAppliedToReference/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppClassCanBeFinal/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppClassIsIncomplete/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppClassNeedsConstructorBecauseOfUninitializedMember/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppClassNeverUsed/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppCompileTimeConstantCanBeReplacedWithBooleanConstant/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppConceptNeverUsed/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppConditionalExpressionCanBeSimplified/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppConstParameterInDeclaration/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppConstValueFunctionReturnType/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppCoroutineCallResolveError/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFAArrayIndexOutOfBounds/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFAConstantConditions/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFAConstantFunctionResult/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFAConstantParameter/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFADeletedPointer/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFAEndlessLoop/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFAInfiniteRecursion/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFAInvalidatedMemory/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFALocalValueEscapesFunction/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFALocalValueEscapesScope/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFALoopConditionNotUpdated/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFAMemoryLeak/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFANotInitializedField/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFANullDereference/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFATimeOver/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFAUnreachableCode/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFAUnreachableFunctionCall/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFAUnreadVariable/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDFAUnusedValue/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDeclarationHidesLocal/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDeclarationHidesUncapturedLocal/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDeclarationSpecifierWithoutDeclarators/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDeclaratorDisambiguatedAsFunction/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDeclaratorNeverUsed/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDeclaratorUsedBeforeInitialization/@EntryIndexedValue" value="ERROR" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDefaultCaseNotHandledInSwitchStatement/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDefaultInitializationWithNoUserConstructor/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDefaultIsUsedAsIdentifier/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDefaultedSpecialMemberFunctionIsImplicitlyDeleted/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDefinitionsOrder/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDeletingVoidPointer/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDependentTemplateWithoutTemplateKeyword/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDependentTypeWithoutTypenameKeyword/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDeprecatedEntity/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDeprecatedOverridenMethod/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDeprecatedRegisterStorageClassSpecifier/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDereferenceOperatorLimitExceeded/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDiscardedPostfixOperatorResult/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDoxygenSyntaxError/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDoxygenUndocumentedParameter/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppDoxygenUnresolvedReference/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEmptyDeclaration/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEnforceCVQualifiersOrder/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEnforceCVQualifiersPlacement/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEnforceDoStatementBraces/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEnforceForStatementBraces/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEnforceFunctionDeclarationStyle/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEnforceIfStatementBraces/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEnforceNestedNamespacesStyle/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEnforceOverridingDestructorStyle/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEnforceOverridingFunctionStyle/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEnforceTypeAliasCodeStyle/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEnforceWhileStatementBraces/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEntityAssignedButNoRead/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEntityUsedOnlyInUnevaluatedContext/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEnumeratorNeverUsed/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEqualOperandsInBinaryExpression/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppEvaluationFailure/@EntryIndexedValue" value="ERROR" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppExplicitSpecializationInNonNamespaceScope/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppExpressionWithoutSideEffects/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppFinalFunctionInFinalClass/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppFinalNonOverridingVirtualFunction/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppForLoopCanBeReplacedWithWhile/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppForwardEnumDeclarationWithoutUnderlyingType/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppFunctionDoesntReturnValue/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppFunctionIsNotImplemented/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppFunctionResultShouldBeUsed/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppFunctionalStyleCast/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppHeaderHasBeenAlreadyIncluded/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppHiddenFunction/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppHidingFunction/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppIdenticalOperandsInBinaryExpression/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppIfCanBeReplacedByConstexprIf/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppImplicitDefaultConstructorNotAvailable/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppIncompatiblePointerConversion/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppIncompleteSwitchStatement/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppInconsistentNaming/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppIntegralToPointerConversion/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppInvalidLineContinuation/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppJoinDeclarationAndAssignment/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppLambdaCaptureNeverUsed/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppLocalVariableMayBeConst/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppLocalVariableMightNotBeInitialized/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppLocalVariableWithNonTrivialDtorIsNeverUsed/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppLongFloat/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppMemberFunctionMayBeConst/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppMemberFunctionMayBeStatic/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppMemberInitializersOrder/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppMismatchedClassTags/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppMissingIncludeGuard/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppMissingKeywordThrow/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppModulePartitionWithSeveralPartitionUnits/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppMsExtAddressOfClassRValue/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppMsExtBindingRValueToLvalueReference/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppMsExtCopyElisionInCopyInitDeclarator/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppMsExtDoubleUserConversionInCopyInit/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppMsExtNotInitializedStaticConstLocalVar/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppMsExtReinterpretCastFromNullptr/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppMultiCharacterLiteral/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppMultiCharacterWideLiteral/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppMustBePublicVirtualToImplementInterface/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppMutableSpecifierOnReferenceMember/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppNoDiscardExpression/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppNodiscardFunctionWithoutReturnValue/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppNonExceptionSafeResourceAcquisition/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppNonExplicitConversionOperator/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppNonExplicitConvertingConstructor/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppNonInlineFunctionDefinitionInHeaderFile/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppNonInlineVariableDefinitionInHeaderFile/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppNotAllPathsReturnValue/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppObjectMemberMightNotBeInitialized/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppOutParameterMustBeWritten/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppParameterMayBeConst/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppParameterMayBeConstPtrOrRef/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppParameterNamesMismatch/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppParameterNeverUsed/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppPassValueParameterByConstReference/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppPointerConversionDropsQualifiers/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppPointerToIntegralConversion/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppPolymorphicClassWithNonVirtualPublicDestructor/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppPossiblyErroneousEmptyStatements/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppPossiblyUninitializedMember/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppPossiblyUnintendedObjectSlicing/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppPrecompiledHeaderIsNotIncluded/@EntryIndexedValue" value="ERROR" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppPrecompiledHeaderNotFound/@EntryIndexedValue" value="ERROR" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppPrintfBadFormat/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppPrintfExtraArg/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppPrintfMissedArg/@EntryIndexedValue" value="ERROR" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppPrintfRiskyFormat/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppPrivateSpecialMemberFunctionIsNotImplemented/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRangeBasedForIncompatibleReference/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedefinitionOfDefaultArgumentInOverrideFunction/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantAccessSpecifier/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantBaseClassAccessSpecifier/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantBaseClassInitializer/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantBooleanExpressionArgument/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantCastExpression/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantComplexityInComparison/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantConditionalExpression/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantConstSpecifier/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantControlFlowJump/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantDereferencingAndTakingAddress/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantElaboratedTypeSpecifier/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantElseKeyword/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantElseKeywordInsideCompoundStatement/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantEmptyDeclaration/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantEmptyStatement/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantExportKeyword/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantFwdClassOrEnumSpecifier/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantInlineSpecifier/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantLambdaParameterList/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantMemberInitializer/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantNamespaceDefinition/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantParentheses/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantQualifier/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantQualifierADL/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantStaticSpecifierOnMemberAllocationFunction/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantStaticSpecifierOnThreadLocalLocalVariable/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantTemplateArguments/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantTemplateKeyword/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantTypenameKeyword/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantVoidArgumentList/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRedundantZeroInitializerInAggregateInitialization/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppReinterpretCastFromVoidPtr/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppRemoveRedundantBraces/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppReplaceMemsetWithZeroInitialization/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppReplaceTieWithStructuredBinding/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppReturnNoValueInNonVoidFunction/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppSmartPointerVsMakeFunction/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppSomeObjectMembersMightNotBeInitialized/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppSpecialFunctionWithoutNoexceptSpecification/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppStaticAssertFailure/@EntryIndexedValue" value="ERROR" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppStaticDataMemberInUnnamedStruct/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppStaticSpecifierOnAnonymousNamespaceMember/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppStringLiteralToCharPointerConversion/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppTabsAreDisallowed/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppTemplateArgumentsCanBeDeduced/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppTemplateParameterNeverUsed/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppTemplateParameterShadowing/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppThrowExpressionCanBeReplacedWithRethrow/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppTooWideScope/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppTooWideScopeInitStatement/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppTypeAliasNeverUsed/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUninitializedDependentBaseClass/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUninitializedNonStaticDataMember/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUnionMemberOfReferenceType/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUnmatchedPragmaEndRegionDirective/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUnmatchedPragmaRegionDirective/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUnnamedNamespaceInHeaderFile/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUnnecessaryWhitespace/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUnsignedZeroComparison/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUnusedIncludeDirective/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUseAlgorithmWithCount/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUseAssociativeContains/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUseAuto/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUseAutoForNumeric/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUseElementsView/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUseEraseAlgorithm/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUseFamiliarTemplateSyntaxForGenericLambdas/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUseRangeAlgorithm/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUseStdSize/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUseStructuredBinding/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUseTypeTraitAlias/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUserDefinedLiteralSuffixDoesNotStartWithUnderscore/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUsingResultOfAssignmentAsCondition/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppVariableCanBeMadeConstexpr/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppVirtualFunctionCallInsideCtor/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppVirtualFunctionInFinalClass/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppVolatileParameterInDeclaration/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppWarningDirective/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppWrongIncludesOrder/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppWrongSlashesInIncludeDirective/@EntryIndexedValue" value="HINT" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppZeroConstantCanBeReplacedWithNullptr/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppZeroValuedExpressionUsedAsNullPointer/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=IdentifierTypo/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=IfStdIsConstantEvaluatedCanBeReplaced/@EntryIndexedValue" value="SUGGESTION" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=StdIsConstantEvaluatedWillAlwaysEvaluateToConstant/@EntryIndexedValue" value="WARNING" type="string" />
-    <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=StringLiteralTypo/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
     <option name="/Default/CodeStyle/CodeFormatting/CppClangFormat/EnableClangFormatSupport/@EntryValue" value="true" type="bool" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_ARGUMENT/@EntryValue" value="true" type="bool" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_BINARY_EXPRESSIONS_CHAIN/@EntryValue" value="true" type="bool" />
@@ -265,12 +21,12 @@
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/BREAK_TEMPLATE_DECLARATION/@EntryValue" value="LINE_BREAK" type="string" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/CASE_BLOCK_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/CONTINUOUS_LINE_INDENT/@EntryValue" value="Double" type="string" />
+    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/EXPORT_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_ACCESS_SPECIFIERS_FROM_CLASS/@EntryValue" value="false" type="bool" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_CASE_FROM_SWITCH/@EntryValue" value="true" type="bool" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_CLASS_MEMBERS_FROM_ACCESS_SPECIFIERS/@EntryValue" value="true" type="bool" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_COMMENT/@EntryValue" value="true" type="bool" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_SIZE/@EntryValue" value="4" type="int" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_STYLE/@EntryValue" value="Space" type="string" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INITIALIZER_BRACES/@EntryValue" value="END_OF_LINE_NO_SPACE" type="string" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INT_ALIGN_EQ/@EntryValue" value="false" type="bool" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INVOCABLE_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
@@ -286,6 +42,7 @@
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/PLACE_ELSE_ON_NEW_LINE/@EntryValue" value="false" type="bool" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/PLACE_NAMESPACE_DEFINITIONS_ON_SAME_LINE/@EntryValue" value="false" type="bool" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/PLACE_WHILE_ON_NEW_LINE/@EntryValue" value="false" type="bool" />
+    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/REQUIRES_EXPRESSION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SIMPLE_BLOCK_STYLE/@EntryValue" value="DO_NOT_CHANGE" type="string" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_CAST_EXPRESSION_PARENTHESES/@EntryValue" value="true" type="bool" />
     <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_COLON_IN_BITFIELD_DECLARATOR/@EntryValue" value="true" type="bool" />
diff --git a/.idea/guppy.iml b/.idea/guppy.iml
deleted file mode 100644
index 6a1cbb8..0000000
--- a/.idea/guppy.iml
+++ /dev/null
@@ -1,8 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<module version="4">
-  <component name="FacetManager">
-    <facet type="Python" name="Python facet">
-      <configuration sdkName="Python 3.12 (guppy)" />
-    </facet>
-  </component>
-</module>
\ No newline at end of file
diff --git a/.idea/jsonSchemas.xml b/.idea/jsonSchemas.xml
deleted file mode 100644
index 9d06c9e..0000000
--- a/.idea/jsonSchemas.xml
+++ /dev/null
@@ -1,26 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="JsonSchemaMappingsProjectConfiguration">
-    <state>
-      <map>
-        <entry key="Compilation Database">
-          <value>
-            <SchemaInfo>
-              <option name="name" value="Compilation Database" />
-              <option name="relativePathToSchema" value="http://clang.llvm.org/docs/JSONCompilationDatabase.html!" />
-              <option name="schemaVersion" value="JSON Schema 7" />
-              <option name="applicationDefined" value="true" />
-              <option name="patterns">
-                <list>
-                  <Item>
-                    <option name="path" value="compile_commands.json" />
-                  </Item>
-                </list>
-              </option>
-            </SchemaInfo>
-          </value>
-        </entry>
-      </map>
-    </state>
-  </component>
-</project>
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
index 1556602..8067168 100644
--- a/.idea/misc.xml
+++ b/.idea/misc.xml
@@ -1,11 +1,5 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
-  <component name="Black">
-    <option name="sdkName" value="Python 3.12 (guppy)" />
-  </component>
-  <component name="CMakePythonSetting">
-    <option name="pythonIntegrationState" value="YES" />
-  </component>
   <component name="CompDBSettings">
     <option name="linkedExternalProjectsSettings">
       <CompDBProjectSettings>
diff --git a/.idea/workspace.xml b/.idea/workspace.xml
new file mode 100644
index 0000000..5bede38
--- /dev/null
+++ b/.idea/workspace.xml
@@ -0,0 +1,145 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="AutoImportSettings">
+    <option name="autoReloadType" value="SELECTIVE" />
+  </component>
+  <component name="BackendCodeEditorMiscSettings">
+    <option name="/Default/Environment/Hierarchy/GeneratedFilesCacheKey/Timestamp/@EntryValue" value="4" type="long" />
+    <option name="/Default/Housekeeping/FeatureSuggestion/FeatureSuggestionManager/DisabledSuggesters/=SwitchToGoToActionSuggester/@EntryIndexedValue" value="true" type="bool" />
+  </component>
+  <component name="CMakeProjectFlavorService">
+    <option name="flavorId" value="CMakePlainProjectFlavor" />
+  </component>
+  <component name="CMakeRunConfigurationManager">
+    <generated />
+  </component>
+  <component name="CMakeSettings">
+    <configurations>
+      <configuration PROFILE_NAME="Debug" ENABLED="true" CONFIG_NAME="Debug" />
+    </configurations>
+  </component>
+  <component name="ChangeListManager">
+    <list default="true" id="531fff0b-c39a-46b8-91a9-7ee840261884" name="Changes" comment="">
+      <change afterPath="$PROJECT_DIR$/src/guppy_vision/src/camera_pub.cpp" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/.gitignore" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/codeStyles/Project.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/codeStyles/Project.xml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/dictionaries/project.xml" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/editor.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/editor.xml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/guppy.iml" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/icon.svg" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/jsonSchemas.xml" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/misc.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/misc.xml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/modules.xml" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/vcs.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/vcs.xml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/src/guppy_vision/CMakeLists.txt" beforeDir="false" afterPath="$PROJECT_DIR$/src/guppy_vision/CMakeLists.txt" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/src/guppy_vision/package.xml" beforeDir="false" afterPath="$PROJECT_DIR$/src/guppy_vision/package.xml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/src/guppy_vision/src/camera_manager.cpp" beforeDir="false" afterPath="$PROJECT_DIR$/src/guppy_vision/src/camera_manager.cpp" afterDir="false" />
+    </list>
+    <option name="SHOW_DIALOG" value="false" />
+    <option name="HIGHLIGHT_CONFLICTS" value="true" />
+    <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
+    <option name="LAST_RESOLUTION" value="IGNORE" />
+  </component>
+  <component name="CompDBLocalSettings">
+    <option name="availableProjects">
+      <map>
+        <entry>
+          <key>
+            <ExternalProjectPojo>
+              <option name="name" value="guppy" />
+              <option name="path" value="$PROJECT_DIR$" />
+            </ExternalProjectPojo>
+          </key>
+          <value>
+            <list>
+              <ExternalProjectPojo>
+                <option name="name" value="guppy" />
+                <option name="path" value="$PROJECT_DIR$" />
+              </ExternalProjectPojo>
+            </list>
+          </value>
+        </entry>
+      </map>
+    </option>
+    <option name="projectSyncType">
+      <map>
+        <entry key="$PROJECT_DIR$" value="RE_IMPORT" />
+      </map>
+    </option>
+  </component>
+  <component name="ExternalProjectsData">
+    <projectState path="$PROJECT_DIR$">
+      <ProjectState />
+    </projectState>
+  </component>
+  <component name="Git.Settings">
+    <option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$" />
+  </component>
+  <component name="MacroExpansionManager">
+    <option name="directoryName" value="CJgJ3Cwd" />
+  </component>
+  <component name="ProjectApplicationVersion">
+    <option name="ide" value="CLion" />
+    <option name="majorVersion" value="2025" />
+    <option name="minorVersion" value="3.3" />
+    <option name="productBranch" value="Classic" />
+  </component>
+  <component name="ProjectColorInfo"><![CDATA[{
+  "customColor": "a60f2dff",
+  "associatedIndex": -1
+}]]></component>
+  <component name="ProjectId" id="3AK2vR2fYii419p2I2e9K9vIlAC" />
+  <component name="ProjectStatusUIService"><![CDATA[{
+  "uiKindId": "Widget"
+}]]></component>
+  <component name="ProjectViewState">
+    <option name="hideEmptyMiddlePackages" value="true" />
+    <option name="showLibraryContents" value="true" />
+  </component>
+  <component name="PropertiesComponent"><![CDATA[{
+  "keyToString": {
+    "ModuleVcsDetector.initialDetectionPerformed": "true",
+    "RunOnceActivity.RadMigrateCodeStyle": "true",
+    "RunOnceActivity.ShowReadmeOnStart": "true",
+    "RunOnceActivity.TerminalTabsStorage.copyFrom.TerminalArrangementManager.252": "true",
+    "RunOnceActivity.cidr.known.project.marker": "true",
+    "RunOnceActivity.readMode.enableVisualFormatting": "true",
+    "RunOnceActivity.typescript.service.memoryLimit.init": "true",
+    "cf.advertisement.text.overridden": "true",
+    "cf.first.check.clang-format": "false",
+    "cidr.known.project.marker": "true",
+    "com.intellij.ml.llm.matterhorn.ej.ui.settings.DefaultModelSelectionForGA.v1": "true",
+    "git-widget-placeholder": "andy/vision__ack",
+    "junie.onboarding.icon.badge.shown": "true",
+    "node.js.detected.package.eslint": "true",
+    "node.js.detected.package.tslint": "true",
+    "node.js.selected.package.eslint": "(autodetect)",
+    "node.js.selected.package.tslint": "(autodetect)",
+    "nodejs_package_manager_path": "npm",
+    "org.rust.first.attach.projects": "true",
+    "to.speed.mode.migration.done": "true",
+    "vue.rearranger.settings.migration": "true"
+  }
+}]]></component>
+  <component name="RunManager">
+    <configuration default="true" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="false" PASS_PARENT_ENVS_2="true">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+  </component>
+  <component name="TaskManager">
+    <task active="true" id="Default" summary="Default task">
+      <changelist id="531fff0b-c39a-46b8-91a9-7ee840261884" name="Changes" comment="" />
+      <created>1772327172470</created>
+      <option name="number" value="Default" />
+      <option name="presentableId" value="Default" />
+      <updated>1772327172470</updated>
+      <workItem from="1772327174297" duration="2908000" />
+    </task>
+    <servers />
+  </component>
+  <component name="TypeScriptGeneratedFilesManager">
+    <option name="version" value="3" />
+  </component>
+</project>
\ No newline at end of file
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 3c695ad..59057cc 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -29,5 +29,6 @@
         "unordered_map": "cpp",
         "valarray": "cpp",
         "vector": "cpp"
-    }
+    },
+    "cmake.ignoreCMakeListsMissing": true
 }
\ No newline at end of file
diff --git a/ros2_env.bash b/ros2_env.bash
new file mode 100644
index 0000000..3950fe1
--- /dev/null
+++ b/ros2_env.bash
@@ -0,0 +1,3 @@
+source /opt/ros/jazzy/setup.bash
+export QT_QPA_PLATFORM=xcb
+export GDK_BACKEND=x11
diff --git a/src/guppy/README.md b/src/guppy/README.md
new file mode 100644
index 0000000..9253606
--- /dev/null
+++ b/src/guppy/README.md
@@ -0,0 +1,8 @@
+# guppy
+This package contains launch files and serves as a "metapackage" for other ROS packages, because it "depends" on all other guppy packages.
+
+An overview of the main package structure can be seen below:
+<img width="666" height="207" alt="image" src="https://github.com/user-attachments/assets/50cc294f-26f6-4131-a712-df3d99d42eeb" />
+
+Additionally, here is a generalized node graph of the entire system:
+<img width="769" height="452" alt="image" src="https://github.com/user-attachments/assets/04ab8a28-3107-4cfc-b2ba-de3d67166fd4" />
diff --git a/src/guppy_control/README.md b/src/guppy_control/README.md
index 912b54f..983d8b8 100644
--- a/src/guppy_control/README.md
+++ b/src/guppy_control/README.md
@@ -94,15 +94,86 @@ typedef struct chassis_controller_params_ {
 
 It also gets handed a pointer to the T200 Interface for control, and a `dt_us` timestep parameter. Control loop frequency in Hertz: `Hz = 1000000 / dt_us`.
 
-#### Math and Control Theory
-The controller handles the following:
-- **Feedforward:**
-    - **Gravity and Buoyancy Force Control:** offsetting the restoring forces of gravity and buoyancy. The sub should have positive buoyancy, which means this generally power the vertical motors to keep the sub down. However, this also handles buoyancy when it is rotated, and vectors accordingly.
-    - **Buoyancy Torque Control**: When upside down, the sub will naturally want to flip back to upright because the center of buoyancy is above the center of gravity. The code seeks to counteract this torque.
-    - **Drag Force**: At a constant velocity, the drag force encountered is equal to the acting force, and increases quadratically with velocity. To counteract this drag, an equal force based on the desired velocity is calculated.
-    - **Drag Moments**: Because the center of pressure is often not coaxial with the center of mass during movement, movements can also cause torques on the sub! This is fixed by multiplying a drag effect matrix with the drag vector to adjust the torques and forces accordingly.
-- **Feedback:**
-    - **Velocity PID**: While the drag feedforward can work for constant velocities, to accelerate between two different velocities (including from a stationary position), we run two PID controllers (one for angular and one for linear) on the velocity error to handle this acceleration and any small innacuracies that may arise.
-    - **Position PID**: When velocity is 0 (or belowe a certain deadband value), the position druft should be zero as well, although there is a chance that it will drift slower than the velocity loop can handle. Because of this, we run a position controller on the linear pose if the velocity on each axis is belowe a deadband. This is part of the "pose nudge."
-    - **Angular PID**: Similar to position, we only run this when angular velocity on each axis is below deadband values. We then calculate quaternion axis error between the last saved state, and run a PID controller on that. This is also part of the "pose nudge."
-- **Thrust Allocation**: Finally, after the feedforward, feedback, and pose nudge terms are summed, they are passed into a Quadratic Programming (QP) solver using the Proxsuite library. This solves a least-squares problem turned into a QP approach with box constraints on the variables. 
\ No newline at end of file
+## Math and Control Theory
+
+Guppy’s control code runs in parallel with the ROS code on the LattePanda Sigma computer and is managed by a ROS node that communicates with it. The highly optimized closed loop control uses the Eigen and Proxsuite C++ libraries in a layered PID approach. The control structure is responsible for several important tasks, which are outlined in each section below.
+
+### Velocity Control
+
+At its very core, the control code subscribes to the commanded velocity topic (`/cmd_vel`) and the current odometry state topic (`/odometry/filtered`) and attempts to match their 6D vectors of velocities. This is done through feedback controllers on each axis’s velocity error coupled with feedforward terms to combat drag. As velocity in each degree of freedom (three horizontal axes and three rotational) increases, drag in that axis also increases (approximately) quadratically.
+
+Because of this, we use traditional PID controllers to handle most of all velocity control (typically acceleration or deceleration), and a small feedforward term based on the calculated drag to hold a constant velocity. While the code has this capability, we have found in our testing with Guppy that we can rely solely on velocity error PID to maintain consistent velocities, as proportional feedback control gives us acceptable tracks. However, the functionality is there, and we intend to tune the controls even further with more testing time in the future.
+
+### Drag Moments
+
+With many AUVs and ROVs, the center of pressure is not coaxial with the center of mass during movement. This means that movements in any linear degree of freedom can additionally cause unwanted torques. In our experience with Guppy, we have not run across this issue in any noticeable way, due to the relatively slow speeds and its large mass. However, as we implement more tuning and additional mechanisms, this will come into play.
+
+To address these unwanted wrenches, the control code incorporates a “drag effect matrix” which is multiplied by the drag vector to adjust the torques and forces accordingly. The columns of the matrix correspond to the axis that is “causing” the unwanted motion, while the rows correspond to where the motion actually occurs. In an idealized scenario, this would be the 6 × 6 identity matrix. However, if motion forward in the X direction causes an unwanted yaw (due to an uncentered drag force), we can adjust the drag effect matrix to counter it like so:
+
+$$
+V_{drag}=
+\begin{bmatrix}
+1  &  0  &  0  &  0  &  0  &  0  \\
+0  &  1  &  0  &  0  &  0  &  0  \\
+0  &  0  &  1  &  0  &  0  &  0  \\
+0  &  0  &  0  &  1  &  0  &  0  \\
+0  &  0  &  0  &  0  &  1  &  0  \\
+0.1&  0  &  0  &  0  &  0  &  1
+\end{bmatrix}
+\begin{bmatrix}
+1 \\
+0 \\
+0 \\
+0 \\
+0 \\
+0
+\end{bmatrix}=
+\begin{bmatrix}
+1 \\
+0 \\
+0 \\
+0 \\
+0 \\
+0.1
+\end{bmatrix}
+$$
+
+The above equation shows how the large 6 × 6 matrix can affect the drag feedforward term with an added 0.1 in yaw offset to counteract the drag moment. This can easily scale with multiple axes, allowing for very precise tuning of the submarine’s motion. Note that all code for Guppy follows the standard ROS axis convention of Z-up and X-forward.
+
+### Restoring Forces
+
+There are several restoring forces that act on Guppy at any given time, primarily gravity and buoyancy. Guppy is very positively buoyant while unpowered and must hold itself underwater using thrust. To maintain pseudo-neutral buoyancy, we calculate the difference between the gravity and buoyancy forces in the Z-axis, and then transform that vector to the AUV’s coordinate frame by multiplying it by the inverse quaternion of the sub’s current orientation. We then add this three-dimensional vector to the existing linear feedforward velocity terms. This allows the sub to stay underwater no matter which way it is rotated!	
+
+This correction only accounts for "neutral buoyancy" though and does not offset the restoring torques that the buoyancy wrench imparts on the sub as well. To combat this, we take the rotated "center of buoyancy" offset from the center of mass, and cross-product this vector with the unrotated buoyancy force from before. This essentially computes the sub's tendency to right itself, and the inverse can be applied to the controller’s outputs to negate this.
+
+### Station Keeping
+
+When we aren’t commanding specific velocities, we need the submarine to hold position unless disabled. This is achieved partially via linear and angular velocity PID, because if the setpoint is zero velocity, the sub will resist motion. However, it will not return to its original position if it does move. We have overcome this by switching to a position based PID controller when the commanded velocity is less than some deadband value. The position value is measured from the fused “odometry” value from the DVL and other sensors like the barometer and cameras. The linear PID is very simple, although the angular feedback can get slightly more complicated.
+
+For angular orientation error, we compute the error quaternion $q_{err}=q^{-1}⨂q_{desired}$ where $q$ is the current orientation state of the submarine, and decompose it into its imaginary parts x, y, and z. For small angular error, these error components can be directly inputted into roll, pitch, and yaw feedback controllers. Again, we use a deadband value to determine when to apply each axis’s feedback, although we must carefully apply them in pairs as rotational motion can be tightly coupled across the axes. This quaternion error approximation breaks down for large angular error, but that state should never occur provided that the initial state is the desired one and that the system is correctly tuned.
+
+Not only does this station keeping allow us to hold our pose, but we also expose a `/reset_holding_pose` service that allows other ROS nodes to change the holding state position. This is used by our navigation actions, which decompose trajectories into a series of staggered waypoints, each of which can be applied to the station keeping code in a timed sequence to follow a path. This is exactly what helped us complete our prequalification run in early May.
+
+### Thrust Allocation
+
+After all these individual components assemble into one large six-dimensional AUV wrench vector, we need to convert that into an eight-dimensional motor thrust vector. To do so, we first converted all motor positions into an 6 × 8 matrix of motor coefficients. This is done by taking their x, y, and z locations as well as their spherical angles φ and θ and calculating the cartesian coefficients of each motor. Next, we solve the following least squares problem:
+
+$$A\overset{\rightharpoonup}{x}=s$$
+
+Where A corresponds to the motor coefficient matrix, $\overset{\rightharpoonup}{x}$ to the eight motor thrusts, and s to the desired output wrench. We solve this using least squares, because we need to constrain the motor outputs on their maximum forwards and reversed thrusts. There is also no guaranteed solution under saturation (at very high speeds or some combined movements), which means that simply solving this matrix equation will often fail. That makes this a box-constrained least-squares problem.
+
+To optimize the solving of this and implement it in an existing library (Proxsuite), we have converted this problem into a Quadratic Programming (QP) problem instead. Many least-squares minimization problems can be rewritten as QP problems, as noted by [S. Caron](https://scaron.info/blog/conversion-from-least-squares-to-quadratic-programming.html). Caron describes how to perform this transfer, where we end up with:
+
+$$\min_{\overset{\rightharpoonup}{x}\in\mathbb{R}^8}\frac{1}{2}\overset{\rightharpoonup}{x}^T H\overset{\rightharpoonup}{x}+g^T\overset{\rightharpoonup}{x}$$
+
+given that:
+
+$$A\overset{\rightharpoonup}{x}=s$$
+
+$$\overset{\rightharpoonup}{T}_lower \le \overset{\rightharpoonup}{x} \le \overset{\rightharpoonup}{T}_upper$$
+
+$$H=A^TWA$$
+
+$$g=-A^TWs$$
+
+…where M and s are still the motor coefficient matrix and desired output wrench. W corresponds to an optional weight matrix to weight one axis more than the other in the solution. The $\overset{\rightharpoonup}{T}$ constants refer to the lower and upper thrust bounds of the T200 thrusters (in SI units). In this form, the QP problem is easily applied directly to Proxsuite, and can take advantage of historical loop data to better inform the next guesses. To prevent “stable” states in which opposing thrusters fire at full force (yet still cancelling out), we add a very small addition where $H=A^TWA+0.1I_8$.
diff --git a/src/guppy_control/config/controller_params.yaml b/src/guppy_control/config/controller_params.yaml
index 9983e7a..a1d7c54 100644
--- a/src/guppy_control/config/controller_params.yaml
+++ b/src/guppy_control/config/controller_params.yaml
@@ -26,7 +26,7 @@ control_chassis:
     pid_gains_vel_angular: [ 1000.0, 0.0, 0.0 ]
 
     pid_gains_pose_linear: [ 300.0, 0.0, 12.0 ]
-    pid_gains_pose_angular: [ 100.0, 0.0, 5.0 ]
+    pid_gains_pose_angular: [ 100.0, 0.0, 10.0 ]
 
     # pid_gains_vel_linear: [ 0.0, 0.0, 0.0 ]
     # pid_gains_vel_angular: [ 0.0, 0.0, 0.0 ]
@@ -49,6 +49,6 @@ control_chassis:
     drag_areas: [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 ]
     water_density: 1000.0 # kg/m^3
     robot_volume: 0.03963 # m^3
-    robot_mass: 34.1 # kg
+    robot_mass: 35.0 # kg
     center_of_buoyancy: [ 0.0, 0.0, -0.08 ]
     qp_epsilon: 0.001
\ No newline at end of file
diff --git a/src/guppy_description/README.md b/src/guppy_description/README.md
new file mode 100644
index 0000000..9624b18
--- /dev/null
+++ b/src/guppy_description/README.md
@@ -0,0 +1,9 @@
+# guppy_description package
+
+This package contains the URDF file with much of Guppy's information and positioning. This also includes sensor positions.
+
+<img width="253" height="101" alt="image" src="https://github.com/user-attachments/assets/b42f51a3-cfc0-40f7-af27-9a45b6027676" />
+
+Guppy in the simulator.
+
+# FOR MORE INFO: see https://github.com/palouserobosub/gncea
diff --git a/src/guppy_localization/CMakeLists.txt b/src/guppy_localization/CMakeLists.txt
index 93699c2..fc49a86 100644
--- a/src/guppy_localization/CMakeLists.txt
+++ b/src/guppy_localization/CMakeLists.txt
@@ -21,12 +21,13 @@ find_package(tf2 REQUIRED)
 find_package(guppy_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(std_srvs REQUIRED)
 
 
 add_executable(barometer_pub src/barometer_pub.cpp)
 add_executable(combine_sensors src/combine.cpp)
 ament_target_dependencies(barometer_pub rclcpp sensor_msgs geometry_msgs guppy_msgs std_msgs)
-ament_target_dependencies(combine_sensors tf2 tf2_geometry_msgs tf2_ros rclcpp sensor_msgs geometry_msgs nav_msgs std_msgs Eigen3)
+ament_target_dependencies(combine_sensors tf2 tf2_geometry_msgs tf2_ros rclcpp sensor_msgs geometry_msgs nav_msgs std_msgs std_srvs Eigen3)
 
 install(TARGETS
   barometer_pub
diff --git a/src/guppy_localization/README.md b/src/guppy_localization/README.md
new file mode 100644
index 0000000..b1639f9
--- /dev/null
+++ b/src/guppy_localization/README.md
@@ -0,0 +1,7 @@
+# guppy_localization
+
+This package contains the sensor fusion code, as well as the CAN acquisition code to read the barometer data. Additionally, these launch files also start up the DVL and IMU.
+
+Several of Guppy’s sensors are COTS sensors we have either bought or inherited from previous teams. For an IMU, we have a sponsored VN-100 from VectorNav Technologies which provides us with high-resolution inertial measurements and orientation data. We also use a WaterLinked DVL-A50 as our Doppler Velocity Log, which provides us with very accurate relative velocity data, as well as dead-reckoning position estimates. Both these sensors communicate directly with our single-board-computer, the LattePanda Sigma from DFRobot.
+
+Additional sensors include two rotary switches from Blue Robotics and bolt-style reed switches from McMaster Carr which serve as generic GPIO inputs and power switches. We also incorporate a Blue Robotics  High-Resolution Depth/Pressure sensor for accurate Z-axis measurements. We also rely on several Logitech USB webcams, as well as several USB and Ethernet Flir/PointGrey high resolution cameras for our vision system.
diff --git a/src/guppy_localization/package.xml b/src/guppy_localization/package.xml
index 152a14b..7a1a4c9 100644
--- a/src/guppy_localization/package.xml
+++ b/src/guppy_localization/package.xml
@@ -11,6 +11,7 @@
 
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>geometry_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>nav_msgs</depend>
diff --git a/src/guppy_localization/src/combine.cpp b/src/guppy_localization/src/combine.cpp
index 7958839..1458fd3 100644
--- a/src/guppy_localization/src/combine.cpp
+++ b/src/guppy_localization/src/combine.cpp
@@ -1,8 +1,10 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/point.hpp"
 #include "sensor_msgs/msg/imu.hpp"
 #include "tf2_ros/transform_broadcaster.hpp"
+#include "std_srvs/srv/empty.hpp"
 
 
 
@@ -17,6 +19,22 @@ class CombineSensors : public rclcpp::Node {
     odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("/odometry/filtered", 10);
     tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
 
+    initial_pose.x = 0.0;
+    initial_pose.y = 0.0;
+    initial_pose.z = 0.0;
+
+    reset_service = this->create_service<std_srvs::srv::Empty>("reset_odom", [&](const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) {
+      initial_pose.x = odom.pose.pose.position.x;
+      initial_pose.y = odom.pose.pose.position.y;
+      initial_pose.z = odom.pose.pose.position.z;
+      initial_orientation = Eigen::Quaterniond(
+        odom.pose.pose.orientation.w,
+        odom.pose.pose.orientation.x,
+        odom.pose.pose.orientation.y,
+        odom.pose.pose.orientation.z
+      );
+    }, 10);
+
     geometry_msgs::msg::TransformStamped msg;
     msg.header.frame_id = "map";
     msg.header.stamp = this->get_clock()->now();
@@ -29,7 +47,7 @@ class CombineSensors : public rclcpp::Node {
   }
 
 void baro_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
-    odom.pose.pose.position.z = msg->pose.pose.position.z;
+    odom.pose.pose.position.z = msg->pose.pose.position.z - initial_pose.z;
 
     odom_pub_->publish(odom);
 
@@ -47,8 +65,8 @@ void baro_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
     position = quat * position;
     twist = quat * twist;
 
-    odom.pose.pose.position.x = position[0];
-    odom.pose.pose.position.y = position[1];
+    odom.pose.pose.position.x = position[0] - initial_pose.x;
+    odom.pose.pose.position.y = position[1] - initial_pose.y;
     // odom.pose.pose.position.z = position[2];
 
     double rx = 0.2;
@@ -124,8 +142,11 @@ void baro_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
   nav_msgs::msg::Odometry odom;
 
   Eigen::Quaterniond initial_orientation;
+  geometry_msgs::msg::Point initial_pose;
   bool has_initial = false;
 
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_service;
+
 };
 
 int main(int argc, char * argv[]) {
diff --git a/src/guppy_moveit/CMakeLists.txt b/src/guppy_moveit/CMakeLists.txt
deleted file mode 100644
index 850f193..0000000
--- a/src/guppy_moveit/CMakeLists.txt
+++ /dev/null
@@ -1,26 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(guppy_moveit)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_package()
diff --git a/src/guppy_moveit/package.xml b/src/guppy_moveit/package.xml
deleted file mode 100644
index 5cea8bb..0000000
--- a/src/guppy_moveit/package.xml
+++ /dev/null
@@ -1,18 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>guppy_moveit</name>
-  <version>0.0.0</version>
-  <description>MoveIT configuration files and descriptions for the arm on the 2026 Guppy AUV.</description>
-  <maintainer email="colewilson.main@gmail.com">Palouse RoboSub</maintainer>
-  <license>MIT</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
diff --git a/src/guppy_msgs/CMakeLists.txt b/src/guppy_msgs/CMakeLists.txt
index f338319..890d270 100644
--- a/src/guppy_msgs/CMakeLists.txt
+++ b/src/guppy_msgs/CMakeLists.txt
@@ -29,6 +29,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/TransformList.msg"
 
   "action/Navigate.action"
+  "action/LaneFollow.action"
   
   DEPENDENCIES builtin_interfaces
   DEPENDENCIES geometry_msgs
diff --git a/src/guppy_msgs/README.md b/src/guppy_msgs/README.md
new file mode 100644
index 0000000..37534a8
--- /dev/null
+++ b/src/guppy_msgs/README.md
@@ -0,0 +1,24 @@
+# guppy_msgs
+
+This package contains ROS message definition files for our custom topic, service, and action types. An overview of our custom types can be seen below:
+
+<img width="666" height="157" alt="image" src="https://github.com/user-attachments/assets/548a58c9-79d6-49b1-9e70-b4c6f98bc014" />
+
+Additionally, here are links to view the message definitions:
+
+### Messages
+- [CanFrame.msg](https://github.com/PalouseRobosub/guppy/blob/d2193f1b1619074df57bbf2f1f53d0de6ce1c5c6/src/guppy_msgs/msg/CanFrame.msg)
+- [CornerDetection.msg](https://github.com/PalouseRobosub/guppy/blob/d2193f1b1619074df57bbf2f1f53d0de6ce1c5c6/src/guppy_msgs/msg/CornerDetection.msg)
+- [CornerDetectionList.msg](https://github.com/PalouseRobosub/guppy/blob/d2193f1b1619074df57bbf2f1f53d0de6ce1c5c6/src/guppy_msgs/msg/CornerDetectionList.msg)
+- [RelativeObjectDetection.msg](https://github.com/PalouseRobosub/guppy/blob/d2193f1b1619074df57bbf2f1f53d0de6ce1c5c6/src/guppy_msgs/msg/RelativeObjectDetection.msg)
+- [State.msg](https://github.com/PalouseRobosub/guppy/blob/d2193f1b1619074df57bbf2f1f53d0de6ce1c5c6/src/guppy_msgs/msg/State.msg)
+- [Toast.msg](https://github.com/PalouseRobosub/guppy/blob/d2193f1b1619074df57bbf2f1f53d0de6ce1c5c6/src/guppy_msgs/msg/Toast.msg)
+- [TransformList.msg](https://github.com/PalouseRobosub/guppy/blob/d2193f1b1619074df57bbf2f1f53d0de6ce1c5c6/src/guppy_msgs/msg/TransformList.msg)
+
+### Services
+- [ChangeState.srv](https://github.com/PalouseRobosub/guppy/blob/d2193f1b1619074df57bbf2f1f53d0de6ce1c5c6/src/guppy_msgs/srv/ChangeState.srv)
+- [SendCan.srv](https://github.com/PalouseRobosub/guppy/blob/d2193f1b1619074df57bbf2f1f53d0de6ce1c5c6/src/guppy_msgs/srv/SendCan.srv)
+- [SetHoldPose.srv](https://github.com/PalouseRobosub/guppy/blob/d2193f1b1619074df57bbf2f1f53d0de6ce1c5c6/src/guppy_msgs/srv/SetHoldPose.srv)
+
+### Actions
+- [Navigate.action](https://github.com/PalouseRobosub/guppy/blob/d2193f1b1619074df57bbf2f1f53d0de6ce1c5c6/src/guppy_msgs/action/Navigate.action)
diff --git a/src/guppy_msgs/action/LaneFollow.action b/src/guppy_msgs/action/LaneFollow.action
new file mode 100644
index 0000000..20c097a
--- /dev/null
+++ b/src/guppy_msgs/action/LaneFollow.action
@@ -0,0 +1,8 @@
+float64 lost_timeout
+---
+bool success
+string message
+---
+bool lane_found
+float64 x_center
+float64 depth
\ No newline at end of file
diff --git a/src/guppy_nav/README.md b/src/guppy_nav/README.md
new file mode 100644
index 0000000..d09f42d
--- /dev/null
+++ b/src/guppy_nav/README.md
@@ -0,0 +1,5 @@
+# guppy_nav
+
+This package houses code for the navigation and movement of Guppy across several waypoints. Primarily, the waypoints are chosen as subsets of the path and then are translated to set holding poses with `/reset_holding_pose` from the `guppy_control` package.
+
+This package will also contain code for the BehaviorTree.CPP libraries and utilize them for structuring the autonomy in XML files.
diff --git a/src/guppy_nav/include/guppy_nav/lane_follow_behavior.hpp b/src/guppy_nav/include/guppy_nav/lane_follow_behavior.hpp
new file mode 100644
index 0000000..a3275e9
--- /dev/null
+++ b/src/guppy_nav/include/guppy_nav/lane_follow_behavior.hpp
@@ -0,0 +1,39 @@
+
+#pragma once
+ 
+#include <behaviortree_ros2/bt_action_node.hpp>
+#include "guppy_msgs/action/lane_follow.hpp"
+
+class LaneFollowBehavior : public BT::RosActionNode<guppy_msgs::action::LaneFollow>{
+    public:
+        LaneFollowBehavior(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) : BT::RosActionNode<guppy_msgs::action::LaneFollow>(name,conf,params){}
+
+    static BT::PortsList providedPorts(){
+        return providedBasicPorts({
+            BT::InputPort<double>("lost_timeout",2.0,"seconds without a lane sighted befoe the action fails")
+
+        });
+    }
+
+    bool setGoal(RosActionNode::Goal& goal)override{
+        getInput("lost_timeout",goal.lost_timeout);
+        return true;
+    }
+
+    BT::NodeStatus onResultReceived(const WrappedResult& wr) override{
+        if (wr.result->success){
+            RCLCPP_INFO(logger(),"LaneFolow succeeded : %s",wr.result->message.c_str());
+            return BT::NodeStatus::SUCCESS;
+        }
+        return BT::NodeStatus::FAILURE;
+    }
+    BT::NodeStatus onFeedback(const std::shared_ptr<const Feedback> feedback) {
+        (void)feedback;
+        return BT::NodeStatus::RUNNING;
+    }
+
+    BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override{
+        RCLCPP_ERROR(logger(), "LaneFollow action error: %d",static_cast<int>(error));
+        return BT::NodeStatus::FAILURE;
+    }
+};
\ No newline at end of file
diff --git a/src/guppy_nav/src/behavior_tree.cpp b/src/guppy_nav/src/behavior_tree.cpp
index 1388401..57e1fdd 100644
--- a/src/guppy_nav/src/behavior_tree.cpp
+++ b/src/guppy_nav/src/behavior_tree.cpp
@@ -17,8 +17,11 @@
 #include "guppy_msgs/msg/state.hpp"
 #include "guppy_nav/face_detection_behavior.hpp"
 
+#include <guppy_nav/lane_follow_behavior.hpp>
+
 #define TICK_MS 20
-#define TREE_NAME "t-shape"
+// #define TREE_NAME "t-shape"
+#define TREE_NAME "LaneFollow" //I CHANGE THIS BACK TO WHATEVVER I GUESS
 
 class NavigationBehaviorTree : public rclcpp::Node {
 public:
@@ -28,15 +31,18 @@ class NavigationBehaviorTree : public rclcpp::Node {
         _change_state_client = std::make_shared<rclcpp::Node>("chage_state_behavior_client");
         _navigation_client = std::make_shared<rclcpp::Node>("navigate_behavior_client");
         _detection_subscriber = std::make_shared<rclcpp::Node>("detection_subscriber");
+        _lane_follow_client = std::make_shared<rclcpp::Node>("lane_follow_behavior_client");
 
         BT::RosNodeParams stateParameters(_change_state_client, "change_state");
         BT::RosNodeParams navigateParameters(_navigation_client, "/navigate");
         BT::RosNodeParams detectionParameters(_detection_subscriber, "/cam/test/detections");
+        BT::RosNodeParams laneFollowParameters(_lane_follow_client, "/lane_follow");
 
         factory.registerNodeType<ChangeStateBehavior>("ChangeState", stateParameters);
         factory.registerNodeType<NavigateBehavior>("Navigate", navigateParameters);
         factory.registerNodeType<FaceDetectionBehavior>("FaceDetection", navigateParameters);
         factory.registerNodeType<AcquireDetection>("AcquireDetection", detectionParameters);
+        factory.registerNodeType<LaneFollowBehavior>("LaneFollow", laneFollowParameters);
 
         _tree = std::make_unique<BT::Tree>(factory.createTreeFromFile("./src/guppy_tasks/resource/" + std::string(TREE_NAME) + ".xml"));
 
@@ -65,6 +71,7 @@ class NavigationBehaviorTree : public rclcpp::Node {
     std::shared_ptr<rclcpp::Node> _change_state_client;
     std::shared_ptr<rclcpp::Node> _navigation_client;
     std::shared_ptr<rclcpp::Node> _detection_subscriber;
+    std::shared_ptr<rclcpp::Node> _lane_follow_client;
 
     rclcpp::Subscription<guppy_msgs::msg::State>::SharedPtr _subscription;
     rclcpp::TimerBase::SharedPtr _timer;
diff --git a/src/guppy_safety/README.md b/src/guppy_safety/README.md
new file mode 100644
index 0000000..cb51008
--- /dev/null
+++ b/src/guppy_safety/README.md
@@ -0,0 +1,4 @@
+# guppy_safety
+This package will contain additional checks and diagnostics for safe operation of the submarine.
+
+Main kill-switch and disable logic is held at the state machine level, and not here. This package will handle heartbeat code and other fault diagnostics to protect the components themselves.
diff --git a/src/guppy_tasks/README.md b/src/guppy_tasks/README.md
new file mode 100644
index 0000000..02c46e8
--- /dev/null
+++ b/src/guppy_tasks/README.md
@@ -0,0 +1,25 @@
+# guppy_tasks
+
+This package contains high level task execution code written in Python. It publishes velocity information to `/cmd_vel/task` and is called from the nav decision tree.
+
+Currently, here is the breakdown on the tasks we are planning to do:
+
+Our main hope for Guppy in RoboSub 2026 was to pre-qualify for competition, which we have successfully accomplished! For this to happen, we had to break the process down into the following sub-goals:
+-	Research and design an AUV concept.
+-	Manufacture the mechanical components of the AUV.
+-	Design and assemble an electrical harness.
+-	Develop and write manual control software.
+-	Develop and write autonomous control software.
+-	Test and iterate on the design.
+
+As we were on a very tight timeline, this was our only primary goal for competition: *have a working submarine that can successfully navigate autonomously under the water*. Anything above and beyond that was fantastic, but not our primary objective. We planned to leave school at the beginning of May with a solid platform for future development, and one that could hopefully get us some points in competition. 
+
+As for concrete task goals in competition, we planned for the tasks that primarily focused on mobility (rather than additional subsystems such as an arm). These were the starting gate (Begin Assessment), the slalom task (Avoid Debris), surfacing inside the octagon (Resupply), Return Home, and (pre) qualification. 
+
+We are mainly aiming for "Core" points for these tasks according to the task capability matrices, although we haven't been able to complete a full points-based analysis because the points for the tasks have not yet been released. To complete some tasks, including the “Advanced” tracks, we need more testing and integration of our vision systems. Currently, most of the tasks we can complete are based on dead-reckoning, however over the summer we hope to finalize the vision system.
+
+Despite the focus on mobility-only tasks, our club had several teams developing additional capabilities. These include torpedoes (Deploy) and a claw (Recon and Resupply). However, due to time constraints, these features will likely have to be pushed to next year. Our rubber-band-powered torpedo prototype has shown extreme potential, and there is a chance we will be able to attempt it at competition. Our claw is also quite far along in its design but needs a bit more development time due to task objects not matching our size expectations after our initial design.
+
+As we plan to re-use Guppy in the future, we will have plenty of time to develop these capabilities over the next year. We plan to move to a much more rapid development/test cycle next year, which will allow us to compete in 2027 with torpedoes and a claw. Our current task capabilities are listed below:
+
+<img width="753" height="258" alt="image" src="https://github.com/user-attachments/assets/19a3a2f3-c05c-4ef5-8f38-4d6f5dc5a04f" />
diff --git a/src/guppy_tasks/guppy_tasks/lanefollowaction.py b/src/guppy_tasks/guppy_tasks/lanefollowaction.py
new file mode 100644
index 0000000..85c3608
--- /dev/null
+++ b/src/guppy_tasks/guppy_tasks/lanefollowaction.py
@@ -0,0 +1,634 @@
+import math
+import numpy as np
+import cv2
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Image, Imu
+from ros_gz_interfaces.msg import Altimeter
+from std_msgs.msg import Bool
+from cv_bridge import CvBridge
+import time
+import threading
+from rclpy.action import ActionServer, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from guppy_msgs.action import LaneFollow
+from nav_msgs.msg import Odometry
+
+#MADE THIS FROM THE CPP VERSION, I NEED THE HARDWARE TOPICS TO SEE IF THIS WILL WORK IRL
+
+def clampd(v,lo,hi):
+    return max(lo,min(hi,v))
+
+def clampi(v,lo,hi):
+    return int(max(lo,min(hi,v)))
+
+def quat_to_euler_xyz(qx, qy, qz, qw):
+    sinr_cosp = 2.0 * (qw * qx + qy * qz)
+    cosr_cosp = 1.0 - 2.0 * (qx * qx + qy * qy)
+    roll = math.atan2(sinr_cosp, cosr_cosp)
+
+    sinp = 2.0 * (qw * qy - qz * qx)
+    if abs(sinp) >= 1.0:
+        pitch = math.copysign(math.pi / 2.0, sinp)
+    else:
+        pitch = math.asin(sinp)
+
+    siny_cosp = 2.0 * (qw * qz + qx * qy)
+    cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz)
+    yaw = math.atan2(siny_cosp, cosy_cosp)
+
+    return roll, pitch, yaw
+
+class Pole:
+    __slots__ = ("cx","cy","area","bbox")
+
+    def __init__(self,cx,cy,area,bbox):
+        self.cx = cx
+        self.cy = cy
+        self.area = area
+        self.bbox = bbox
+
+class LaneNavigator(Node):
+    def __init__(self):
+        super().__init__("lane_navigator_py")
+        self.declare_parameter("enabled",False)
+        self.declare_parameter("enable_debug",True)
+        self.declare_parameter("debug_show_every_n_frames",2)
+
+        self.declare_parameter("target_depth",1.0)
+        self.declare_parameter("depth_increases_down",True)
+        self.declare_parameter("kp_depth",4.0)
+        self.declare_parameter("ki_depth",0.1)
+        self.declare_parameter("kd_depth",2.0)
+        self.declare_parameter("max_heave_cmd",2.0)
+
+        self.declare_parameter("kp_roll",3.0)
+        self.declare_parameter("kd_roll",0.8)
+        self.declare_parameter("kp_pitch",3.0)
+        self.declare_parameter("kd_pitch",0.8)
+        self.declare_parameter("max_att_cmd",2.0)
+
+        self.declare_parameter("kp_yaw_px",0.006)
+        self.declare_parameter("kd_yaw",0.05)
+        self.declare_parameter("k_lane_theta",0.6)
+        self.declare_parameter("max_yaw",1.2)
+        self.declare_parameter("search_yaw",0.35)
+        self.declare_parameter("lane_lookahead_frac",0.65)
+
+        self.declare_parameter("forward_speed",0.35)
+        self.declare_parameter("forward_speed_search",0.05)
+
+        self.declare_parameter("hsv_red1_low",[0,100,100])
+        self.declare_parameter("hsv_red1_high",[10,255,255])
+        self.declare_parameter("hsv_red2_low",[170,100,100])
+        self.declare_parameter("hsv_red2_high",[180,255,255])
+        self.declare_parameter("hsv_white_low",[0,0,55])
+        self.declare_parameter("hsv_white_high",[179,45,255])
+
+        self.declare_parameter("min_contour_area",300)
+        self.declare_parameter("min_aspect",2.0)
+        self.declare_parameter("blur_ksize",5)
+        self.declare_parameter("morph_ksize",5)
+        self.declare_parameter("max_poles_each_color",6)
+        self.declare_parameter("publish_debugimages", True)
+
+        p = self.get_parameter
+        self.debug_ = p("enable_debug").value
+        self.publish_debug = p("publish_debugimages").value
+        self.debug_every_n_ = max(1,int(p("debug_show_every_n_frames").value))
+        
+        self.target_depth_ = p("target_depth").value
+        self.depth_incrases_down_= p("depth_increases_down").value
+        # self.kp_depth_ = p("kp_depth").value
+        # self.kd_depth = p("max_heave_cmd").value
+        self.kp_depth_ = p("kp_depth").value
+        self.ki_depth_ = p("ki_depth").value
+        self.kd_depth_ = p("kd_depth").value
+        self.max_heave_cmd_ = p("max_heave_cmd").value
+
+        self.kp_roll_ = p("kp_roll").value
+        self.kd_roll_ = p("kd_roll").value
+        self.kp_pitch = p("kp_pitch").value
+        self.kd_pitch = p("kd_pitch").value
+        self.max_att_cmd_ = p("max_att_cmd").value
+
+        self.kp_yaw_px_ = p("kp_yaw_px").value
+        self.kd_yaw_ = p("kd_yaw").value
+        self.k_lane_theta_ = p("k_lane_theta").value
+        self.max_yaw_ = p("max_yaw").value
+        self.search_yaw_ = p("search_yaw").value
+        self.lane_lookahead_frac_ = p("lane_lookahead_frac").value
+        self.forward_speed_ = p("forward_speed").value
+        self.forward_speed_search_ = p("forward_speed_search").value
+        self.min_area_ = p("min_contour_area").value
+        self.min_aspect_ = p("min_aspect").value
+        self.blur_ksize_ = p("blur_ksize").value
+        self.morph_ksize_ = p("morph_ksize").value
+        self.max_poles_each_color_ = p("max_poles_each_color").value
+
+        self.red1_low_ = np.array(p("hsv_red1_low").value, dtype=np.uint8)
+        self.red1_high_ = np.array(p("hsv_red1_high").value, dtype=np.uint8)
+        self.red2_low_ = np.array(p("hsv_red2_low").value, dtype=np.uint8)
+        self.red2_high_ = np.array(p("hsv_red2_high").value, dtype=np.uint8)
+        self.white_low_ = np.array(p("hsv_white_low").value, dtype=np.uint8)
+        self.white_high_ = np.array(p("hsv_white_high").value, dtype=np.uint8)
+
+        # self.bridge_ = CvBridge()
+        # self.was_enabled_ = False
+
+        self.bridge_ = CvBridge()
+
+        self._goal_active = False
+        self._was_active = False
+        self._goal_lock = threading.Lock()
+        self._current_lost_timeout = 2.0
+        self._last_lane_ok = False
+        self._last_x_center = 0.0
+        
+        self.image_ = None
+        self.have_image_ = False
+        self.depth_ = 0.0
+        self.have_depth_ = False
+        self.roll_ = self.pitch_ = self.yaw_ = 0.0
+        self.roll_rate_ = self.pitch_rate_ = self.yaw_rate_ = 0.0
+        self.have_imu_ = False
+
+        self.integral_depth_ = 0.0
+        self.prev_depth_error_ = 0.0
+
+        self.kf_inited = False
+        self.kf_x_ = 0.0
+        self.kf_v_ = 0.0
+        self.P00_ = self.P11_ = 1.0
+        self.P01_ = self.P10_ = 0.0
+        self.q_pos_ = 50.0
+        self.q_vel_ = 200.0
+        self.r_meas_ = 400.0
+
+        # self.frame_count_ = 0
+        # self.prev_time_ = self.get_clock().now()
+
+        # self.cmd_pub_ = self.create_publisher(Twist,"/cmd_vel",10)
+
+        self.frame_count_ = 0
+        self.prev_time_ = self.get_clock().now()
+        # self._cb_group = ReentrantCallbackGroup()
+        self._cb_group = ReentrantCallbackGroup()
+        self._action_server = ActionServer(
+            self,
+            LaneFollow,
+            "lane_follow",
+            execute_callback = self.execute_callback,
+            goal_callback = self.goal_callback,
+            cancel_callback=self.cancel_callback,
+            callback_group = self._cb_group,
+        )
+        self.cmd_pub_ = self.create_publisher(Twist, "/cmd_vel/task", 10)
+        self.status_pub_ = self.create_publisher(Bool,"~/lane_found",10)
+
+        self.create_subscription(
+            Image, "/cube/image_raw", self.image_cb, qos_profile_sensor_data
+        )
+        # self.create_subscription(
+        #     Altimeter, "/altimeter", self.alt_cb, qos_profile_sensor_data
+        # )
+        # self.create_subscription(
+        #     Odometry, "/odometry/filtered", self.alt_cb, qos_profile_sensor_data
+        # )
+        # self.create_subscription(
+        #     Imu, "/odometry/filtered", self.imu_cb, qos_profile_sensor_data
+        # )
+        self.create_subscription(
+            Odometry, "/odometry/filtered", self.odom_cb, qos_profile_sensor_data
+        )
+
+        if self.publish_debug:
+            self.dbg_rgb_pub_ = self.create_publisher(Image, "/lane_debug/rgb", 1)
+            self.dbg_red_pub_ = self.create_publisher(Image, "/lane_debug/red_mask", 1)
+            self.dbg_white_pub_ = self.create_publisher(Image, "/lane_debug/white_mask", 1)
+            self.dbg_overlay_pub_ = self.create_publisher(Image, "/lane_debug/overlay", 1)
+
+        # self.timer_ = self.create_timer(0.01,self.control_loop)
+        self.timer_ = self.create_timer(0.01, self.control_loop, callback_group=self._cb_group)
+
+        self.get_logger().info(
+            f"lane_navigator_py running (100 Hz). "
+            f"publish_debugimages={self.publish_debug} debug_every_n={self.debug_every_n_}"
+        )
+
+    def image_cb(self,msg: Image):
+        try:
+            self.image_ = self.bridge_.imgmsg_to_cv2(msg,desired_encoding="bgr8")
+            self.have_image_ = True
+        except Exception as e:
+            self.get_logger().warn(f"cv_bridge conversion failed: {e}", throttle_duration_sec=2.0)
+
+    def goal_callback(self,goal_request):
+        with self._goal_lock:
+            if self._goal_active:
+                self.get_logger().warn("LaneFollow goal rejected: already active")
+                return GoalResponse.REJECT
+        return GoalResponse.ACCEPT
+            
+            
+    def cancel_callback(self,goal_handle):
+        self.get_logger().info("LaneFollow cancel requested")
+        return CancelResponse.ACCEPT
+    
+    async def execute_callback(self,goal_handle):
+        self._current_lost_timeout = goal_handle.request.lost_timeout
+        with self._goal_lock:
+            self._goal_active = True
+        
+        feedback = LaneFollow.Feedback()
+        result = LaneFollow.Result()
+        last_seen = time.monotonic() 
+        rate_hz = 10.0
+
+        try:
+            while rclpy.ok():
+                if goal_handle.is_cancel_requested:
+                    goal_handle.canceled()
+                    result.success = False
+                    result.message = "canceled"
+                    return result
+            
+                feedback.lane_found = self._last_lane_ok
+                feedback.x_center = self._last_x_center
+                feedback.depth = self.depth_
+                goal_handle.publish_feedback(feedback)
+                if self._last_lane_ok:
+                    last_seen = time.monotonic()
+                elif time.monotonic() - last_seen > self._current_lost_timeout:
+                    goal_handle.abort()
+                    result.success = False
+                    result.message = "lane lost past timeout"
+                    return result
+                
+                time.sleep(1.0 /rate_hz)
+            goal_handle.abort()
+            result.success = False
+            result.message = "rclpy shutdown"
+            return result
+        finally:
+            with self._goal_lock:
+                self._goal_active = False
+
+
+    def alt_cb(self,msg: Odometry):
+        # z = float(msg.vertical_position)
+        z = float(msg.pose.pose.position.z)
+        self.depth_ = z if self.depth_incrases_down_ else -z
+        self.have_depth_ = True
+
+    #XYZ NEEDS TO BE SWAPPED TO WORK WITH GUPPY XYZ
+    def imu_cb(self,msg:Imu):
+        q = msg.orientation
+        self.roll_, self.pitch_,self.yaw_= quat_to_euler_xyz(q.x, q.y, q.z, q.w)
+        self.roll_rate_ = msg.angular_velocity.x
+        self.pitch_rate_ = msg.angular_velocity.y
+        self.yaw_rate_ = msg.angular_velocity.z
+        self.have_imu_ = True
+
+    #ODOM CALLBACK
+    def odom_cb(self, msg: Odometry):
+        z = float(msg.pose.pose.position.z)
+        self.depth_ = z if self.depth_incrases_down_ else -z
+        self.have_depth_ = True
+
+        q = msg.pose.pose.orientation
+        self.roll_, self.pitch_, self.yaw_ = quat_to_euler_xyz(q.x, q.y, q.z, q.w)
+
+        self.roll_rate_ = msg.twist.twist.angular.x
+        self.pitch_rate_ = msg.twist.twist.angular.y
+        self.yaw_rate_ = msg.twist.twist.angular.z
+        self.have_imu_ = True
+
+    def pole_centroids(self,mask):
+        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+        poles = []
+        for c in contours:
+            area = cv2.contourArea(c)
+            if area < self.min_area_:
+                continue
+            x,y,w,h = cv2.boundingRect(c)
+            if w <= 0:
+                continue
+            aspect = h / float(w)
+            if aspect < self.min_aspect_:
+                continue
+            M = cv2.moments(c)
+            if abs(M["m00"]) < 1e-6:
+                continue
+            cx = int(M["m10"] / M["m00"])
+            cy = int(M["m01"] / M["m00"])
+            poles.append(Pole(cx, cy, area, (x, y, w, h)))
+        poles.sort(key=lambda p: (-p.cy, -p.area))
+        return poles[: self.max_poles_each_color_]
+    
+    def pick_best_pair(self,reds,whites):
+        if not reds or not whites:
+            return None, None
+        NR = min(3,len(reds))
+        NW = min(3,len(whites))
+        best_score = float("inf")
+        best_r = best_w = None
+        for i in range(NR):
+            for j in range(NW):
+                r,w = reds[i],whites[j]
+                if r.cx >= w.cx:
+                    continue
+                dy = abs(r.cy - w.cy)
+                area_bonus = 0.0005 * (r.area + w.area)
+                score = dy - area_bonus
+                if score < best_score:
+                    best_score = score
+                    best_r , best_w = r,w
+        return best_r , best_w
+    
+    @staticmethod
+    def fit_line(poles):
+        Syy = Sy = S1 = Sxy = Sx = 0.0
+        for p in poles:
+            y,x = float(p.cy),float(p.cx)
+            Syy += y*y
+            Sy += y
+            S1 += 1.0
+            Sxy += x * y
+            Sx += x
+        det = Syy * S1 - Sy * Sy
+        if abs(det) < 1e-9:
+            return 0.0, Sx / max(1.0,S1)
+        a = (Sxy * S1 - Sx * Sy) / det
+        b = (Syy * Sx - Sy * Sxy) / det
+        return a,b
+    
+    def detect_lane(self,bgr):
+        img = bgr
+        if self.blur_ksize_ > 1:
+            k = self.blur_ksize_ if self.blur_ksize_ % 2 == 1 else self.blur_ksize_ + 1
+            img = cv2.GaussianBlur(img,(k,k),0.0)
+        hsv = cv2.cvtColor(img,cv2.COLOR_BGR2HSV)
+        red1 = cv2.inRange(hsv, self.red1_low_,self.red1_high_)
+        red2 = cv2.inRange(hsv,self.red2_low_,self.red2_high_)
+        red_mask = cv2.bitwise_or(red1, red2)
+        white_mask = cv2.inRange(hsv, self.white_low_, self.white_high_)
+
+        mk = self.morph_ksize_ if self.morph_ksize_ % 2 == 1 else self.morph_ksize_ + 1
+        mk = max(1, mk)
+        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (mk, mk))
+        red_mask = cv2.morphologyEx(red_mask, cv2.MORPH_OPEN, kernel, iterations=1)
+        red_mask = cv2.morphologyEx(red_mask, cv2.MORPH_CLOSE, kernel, iterations=2)
+        white_mask = cv2.morphologyEx(white_mask, cv2.MORPH_OPEN, kernel, iterations=1)
+        white_mask = cv2.morphologyEx(white_mask, cv2.MORPH_CLOSE, kernel, iterations=2)
+        overlay = bgr.copy()
+        overlay[red_mask > 0] = (0,0,255)
+        overlay[white_mask > 0] = (255,255,255)
+        
+        red_poles = self.pole_centroids(red_mask)
+        white_poles = self.pole_centroids(white_mask)
+
+        for p in red_poles:
+            x,y,w,h = p.bbox
+            cv2.rectangle(overlay, (x,y), (x+w,y+h),(0,0,255),2)
+            cv2.circle(overlay, (p.cx,p.cy),3, (200,200,200),-1)
+
+        best_r , best_w = self.pick_best_pair(red_poles,white_poles)
+        if best_r is None:
+            return False, None, None, red_mask, white_mask, overlay
+        
+        mid = ((best_r.cx + best_w.cx) // 2, (best_r.cy + best_w.cy) // 2)
+        cv2.circle(overlay,mid,8,(0,255,255),-1)
+        cv2.line(overlay, (best_r.cx, best_r.cy), (best_w.cx , best_w.cy), (0, 255, 255), 2)
+        x_center = float(mid[0])
+        lane_theta =0.0
+
+        if len(red_poles) < 2 or len(white_poles) < 2:
+            return True, x_center, lane_theta, red_mask, white_mask, overlay
+        
+        H,W = bgr.shape[:2]
+        y_look = clampi(self.lane_lookahead_frac_ * H,0,H-1)
+        ar,br = self.fit_line(red_poles)
+        aw,bw = self.fit_line(white_poles)
+        x_left = ar*y_look + br
+        x_right = aw * y_look + bw
+
+        if x_left >= x_right:
+            return True, x_center, lane_theta, red_mask, white_mask, overlay
+        
+        x_center = clampd(0.5 * (x_left + x_right), 0.0, float(W-1))
+        a_center = 0.5 * (ar + aw)
+        lane_theta = math.atan(a_center)
+
+        def draw_xy(a,b,color):
+            y0,y1 = 0, H-1
+            x0 = clampi(a*y0 + b,-2000,2000)
+            x1 = clampi(a*y1 + b, -2000,2000)
+            cv2.line(overlay,(x0,y0),(x1,y1),color,2)
+
+        draw_xy(ar,br,(0,0,255))
+        draw_xy(aw,bw,(220,220,220))
+        cv2.circle(overlay,(int(x_center),y_look),6,(0,255,0),-1)
+        cv2.line(overlay,(W // 2, 0), (W // 2 , H-1), (0,255,255),1)
+        return True, x_center, lane_theta, red_mask, white_mask, overlay
+    
+    def kf_reset(self,x0):
+        self.kf_inited = True
+        self.kf_x_ = x0
+        self.kf_v_ = 0.0
+        self.P00_, self.P01_ = 500.0, 0.0
+        self.P10_, self.P11_ = 0.0, 500.0
+
+    def kf_predict(self,dt):
+        self.kf_x_ = self.kf_x_ + dt * self.kf_v_
+        A00, A01 = 1.0,dt
+        A10, A11 = 0.0,1.0
+        P00, P01, P10, P11 = self.P00_, self.P01_, self.P10_, self.P11_
+        AP00 = A00 * P00 + A01 * P10
+        AP01 = A00 * P01 + A01 * P11
+        AP10 = A10 * P00 + A11 * P10
+        AP11 = A10 * P01 + A11 * P11
+
+        # newP00 = AP00 * AP00 + AP01 * A01
+        newP00 = AP00 * A00 + AP01 * A01
+        newP01 = AP00 * A10 + AP01 * A11
+        newP10 = AP10 * A00 + AP11 * A01
+        newP11 = AP10 * A10 + AP11 * A11
+
+        newP00 += self.q_pos_ * dt * dt
+        newP11 += self.q_vel_*dt
+        self.P00_, self.P01_, self.P10_, self.P11_ = newP00, newP01, newP10, newP11
+
+    def kf_update(self,z_meas):
+        y = z_meas - self.kf_x_
+        S = self.P00_ + self.r_meas_
+        if S < 1e-9:
+            return
+        K0 = self.P00_ / S
+        K1 = self.P10_ / S
+
+        self.kf_x_ = self.kf_x_ + K0 * y
+        self.kf_v_ = self.kf_v_ + K1 * y
+        P00, P01, P10, P11 = self.P00_, self.P01_, self.P10_, self.P11_
+        self.P00_ = (1.0 - K0) * P00
+        self.P01_ = (1.0 - K0) * P01
+        self.P10_ = P10 - K1 * P00
+        self.P11_ = P11 - K1 * P01
+
+    def publish_dbg_images(self,rgb, red_mask, white_mask, overlay):
+        if not self.publish_debug:
+            return
+        stamp = self.get_clock().now().to_msg()
+        def to_bgr(img):
+            m = self.bridge_.cv2_to_imgmsg(img,encoding ="bgr8")
+            m.header.stamp = stamp
+            m.header.frame_id = "camera_link" #THIS NEEDS TO BE CHANGED BECAUSE IDK WHAT THE FRAME FOR THE CAMERA IS
+            return m
+        
+        def to_mono(img):
+            m = self.bridge_.cv2_to_imgmsg(img,encoding="mono8")
+            m.header.stamp = stamp
+            m.header.frame_id = "camera_link"#THIS ALSO NEEDS TO BE CHANGED BECAUSE IDK WHAT THE FRAME FOR THE CAMERA IS
+            return m
+        
+        self.dbg_rgb_pub_.publish(to_bgr(rgb))
+        self.dbg_red_pub_.publish(to_mono(red_mask))
+        self.dbg_white_pub_.publish(to_mono(white_mask))
+        self.dbg_overlay_pub_.publish(to_bgr(overlay))
+
+    def control_loop(self):
+        # enabled = self.get_parameter("enabled").value
+        # if not enabled:
+        #     if self.was_enabled_:
+        #         self.cmd_pub_.publish(Twist())
+        #         self.was_enabled_ = False
+        #     return
+        # self.was_enabled_ = True
+        if not self._goal_active:
+            if self._was_active:
+                self.cmd_pub_.publish(Twist())
+                self._was_active = False
+            return
+        self._was_active = True
+
+        #UNCOMMENT THIS WHEN YOU HAVE IMU/DEPTH SENSOR/ALTIMETER (STILL USES GAZEBO TOPICS)
+        # if not (self.have_image_ and self.have_depth_ and self.have_imu_):
+        #     return
+
+        #SET IT TO THIS WHEN TESTING WITH ONLY CAMERA/WEBCAM
+        if not self.have_image_:
+            return
+    
+        now = self.get_clock().now()
+        dt = (now - self.prev_time_).nanoseconds * 1e-9
+        if dt <= 1e-6:
+            return
+        self.prev_time_ = now
+
+        if self.kf_inited:
+            self.kf_predict(dt)
+
+        depth_error = self.target_depth_ - self.depth_
+        self.integral_depth_ += depth_error * dt
+        derivative_depth = (depth_error - self.prev_depth_error_) / dt
+        self.prev_depth_error_ = depth_error
+
+        # heave = (self.kp_depth_ * depth_error + self.ki_depth * self.integral_depth_ + self.kd_depth * derivative_depth)
+        # heave = clampd(heave, -self.max_heave_cmd, self.max_heave_cmd)
+        heave = (self.kp_depth_ * depth_error + self.ki_depth_ * self.integral_depth_ + self.kd_depth_ * derivative_depth)
+        heave = clampd(heave, -self.max_heave_cmd_, self.max_heave_cmd_)
+
+        roll_cmd = self.kp_roll_ * (0.0 - self.roll_)+ self.kd_roll_ * (0.0 - self.roll_rate_)
+        pitch_cmd = self.kp_pitch * (0.0 - self.pitch_) + self.kd_pitch * (0.0 - self.pitch_rate_)
+        roll_cmd = clampd(roll_cmd, -self.max_att_cmd_, self.max_att_cmd_)
+        pitch_cmd = clampd(pitch_cmd, -self.max_att_cmd_, self.max_att_cmd_)
+
+        # lane_ok, x_center, lane_theta, red_mask, white_mask, overlay = self.detect_lane(self.image_)
+        lane_ok, x_center, lane_theta, red_mask, white_mask, overlay = self.detect_lane(self.image_)
+        self._last_lane_ok = lane_ok
+        self._last_x_center = x_center if lane_ok else self._last_x_center
+
+
+        if lane_ok:
+            if not self.kf_inited:
+                self.kf_reset(x_center)
+            else:
+                self.kf_update(x_center)
+            x_center = self.kf_x_
+        else:
+            y_draw = int(0.65 * self.image_.shape[0])
+            cv2.circle(overlay,(int(self.kf_x_), y_draw), 6, (255, 255, 0), -1)
+        
+        self.frame_count_ += 1
+
+        if self.publish_debug and (self.frame_count_ % self.debug_every_n_ == 0):
+            self.publish_dbg_images(self.image_,red_mask,white_mask,overlay)
+
+        self.status_pub_.publish(Bool(data=lane_ok))
+        cmd = Twist()
+        cmd.linear.z = heave
+        cmd.angular.x = roll_cmd
+        cmd.angular.y = pitch_cmd
+        w = self.image_.shape[1]
+        img_cx = 0.5*w
+
+        if not lane_ok:
+            cmd.linear.x = self.forward_speed_search_
+            cmd.angular.z = clampd(self.search_yaw_ + (-self.kd_yaw_ * self.yaw_rate_), -self.max_yaw_, self.max_yaw_)
+            self.cmd_pub_.publish(cmd)
+            if self.debug_ and (self.frame_count_ % self.debug_every_n_ == 0):
+                self.get_logger().info(f"[SEARCH] x={cmd.linear.x:.2f} z={cmd.linear.z:+.2f} " f"r={cmd.angular.x:+.2f} p={cmd.angular.y:+.2f} yaw={cmd.angular.z:+.2f} " f"depth={self.depth_:.2f}")
+            return
+
+        error_px = img_cx - x_center
+        yaw_track = self.kp_yaw_px_ * error_px
+        yaw_ff = -self.k_lane_theta_ * lane_theta
+        yaw_damp = -self.kd_yaw_ * self.yaw_rate_
+        yaw_cmd = clampd(yaw_track + yaw_ff + yaw_damp, -self.max_yaw_, self.max_yaw_)
+        cmd.linear.x = self.forward_speed_
+        cmd.angular.z = yaw_cmd
+        self.cmd_pub_.publish(cmd)
+        if self.debug_ and (self.frame_count_ % self.debug_every_n_ == 0):
+            self.get_logger().info(
+                f"[LANE] x={cmd.linear.x:.2f} z={cmd.linear.z:+.2f} "
+                f"r={cmd.angular.x:+.2f} p={cmd.angular.y:+.2f} yaw={cmd.angular.z:+.2f} "
+                f"err_px={error_px:+.1f} theta={lane_theta:+.3f} depth={self.depth_:.2f}"
+            )
+def main(args =None):
+    # rclpy.init(args=args)
+    # node = LaneNavigator()
+    # try:
+    #     rclpy.spin(node)
+    # except KeyboardInterrupt:
+    #     pass
+    # finally:
+    #     node.destroy_node()
+    #     rclpy.shutdown()
+    rclpy.init(args = args)
+    node = LaneNavigator()
+    executor = rclpy.executors.MultiThreadedExecutor()
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()
+
+
+
+
+
+
+
+
+
+
diff --git a/src/guppy_tasks/lanefollowaction.md b/src/guppy_tasks/lanefollowaction.md
new file mode 100644
index 0000000..cc0ca42
--- /dev/null
+++ b/src/guppy_tasks/lanefollowaction.md
@@ -0,0 +1,33 @@
+```
+ros2 run usb_cam usb_cam_node_exe --ros-args \
+  -r /image_raw:=/cube/image_raw \
+  -p video_device:=/dev/video0 \
+  -p image_width:=640 \
+  -p image_height:=480 \
+  -p pixel_format:=mjpeg2rgb \
+  -p framerate:=30.0
+```
+
+```
+ros2 run guppy_tasks lanefollowaction
+```
+
+
+```
+ros2 action list
+ros2 action info /lane_follow
+ros2 action send_goal /lane_follow guppy_msgs/action/LaneFollow "{lost_timeout: 2.0}" --feedback
+```
+
+```
+ros2 run rqt_image_view rqt_image_view
+```
+
+```
+ros2 param set /lane_navigator_py hsv_red1_low "[0, 100, 100]"
+ros2 param set /lane_navigator_py hsv_red1_high "[10, 255, 255]"
+ros2 param set /lane_navigator_py hsv_white_low "[0, 0, 55]"
+ros2 param set /lane_navigator_py hsv_white_high "[179, 45, 255]"
+ros2 param set /lane_navigator_py min_aspect 0.3
+ros2 param set /lane_navigator_py min_contour_area 100
+```
\ No newline at end of file
diff --git a/src/guppy_tasks/package.xml b/src/guppy_tasks/package.xml
index de25b0e..c552c44 100644
--- a/src/guppy_tasks/package.xml
+++ b/src/guppy_tasks/package.xml
@@ -7,6 +7,18 @@
   <maintainer email="colewilson.main@gmail.com">Palouse RoboSub</maintainer>
   <license>MIT</license>
 
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>ros_gz_interfaces</depend>
+  <depend>cv_bridge</depend>
+  <depend>guppy_msgs</depend>
+  <depend>nav_msgs</depend>
+  <exec_depend>python3-opencv</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-py-trees</exec_depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>
diff --git a/src/guppy_tasks/resource/LaneFollow.xml b/src/guppy_tasks/resource/LaneFollow.xml
new file mode 100644
index 0000000..d8d2f11
--- /dev/null
+++ b/src/guppy_tasks/resource/LaneFollow.xml
@@ -0,0 +1,12 @@
+<root BTCPP_format="4">
+    <BehaviorTree ID="LaneCourse">
+        <Fallback>
+            <Sequence>
+                <LaneFollow lost_timeout="2.0"/>
+                <ChangeState state="1"/>
+            </Sequence>
+            <ChangeState state="5"/>
+        </Fallback>
+    </BehaviorTree>
+
+</root>
\ No newline at end of file
diff --git a/src/guppy_tasks/setup.py b/src/guppy_tasks/setup.py
index 61b3bba..25e6c4c 100644
--- a/src/guppy_tasks/setup.py
+++ b/src/guppy_tasks/setup.py
@@ -24,6 +24,7 @@
     },
     entry_points={
         'console_scripts': [
+            'lanefollowaction = guppy_tasks.lanefollowaction:main',
         ],
     },
 )
diff --git a/src/guppy_vision/CMakeLists.txt b/src/guppy_vision/CMakeLists.txt
index 29dcb1a..98f78db 100644
--- a/src/guppy_vision/CMakeLists.txt
+++ b/src/guppy_vision/CMakeLists.txt
@@ -21,9 +21,25 @@ find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(image_transport REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
+
+# Spinnaker paths
+set(SPINNAKER_INCLUDE_DIR /opt/spinnaker/include)
+set(SPINNAKER_LIBRARY /opt/spinnaker/lib/libSpinnaker.so)
+
+# Create imported library target
+add_library(Spinnaker::Spinnaker SHARED IMPORTED)
+
+set_target_properties(Spinnaker::Spinnaker PROPERTIES
+        IMPORTED_LOCATION ${SPINNAKER_LIBRARY}
+        INTERFACE_INCLUDE_DIRECTORIES ${SPINNAKER_INCLUDE_DIR}
+)
+
+add_executable(camera_pub src/camera_pub.cpp)
+ament_target_dependencies(camera_pub rclcpp sensor_msgs image_transport)
+
+target_link_libraries(camera_pub
+        Spinnaker::Spinnaker
+)
 
 add_executable(webcam_pub src/webcam_pub.cpp)
 add_executable(pnp src/pnp.cpp)
@@ -40,6 +56,8 @@ install(TARGETS
   aprilrepub
   pnp
   slam
+  camera_pub
+  
   DESTINATION lib/${PROJECT_NAME})
 
 # Install Python modules
diff --git a/src/guppy_vision/README.md b/src/guppy_vision/README.md
new file mode 100644
index 0000000..b53bfee
--- /dev/null
+++ b/src/guppy_vision/README.md
@@ -0,0 +1,21 @@
+# guppy_vision
+
+This package houses all of the main code to drive the vision system on Guppy.
+
+## AprilTag Recognition
+
+Before we train a YOLO model, we are using AprilTag detection instead of emoji glyphs. This allows us to test quickly without needing the full pipeline.
+
+ALl the `apriltag` node does is publish four corners and fiducial ID, it does not perform any pose estimation itself.
+
+## Webcam Publisher
+
+This node simply publishes USB webcams as a ROS image topic. This is intended as a temporary measure until the Flir cameras are mounted externally, which have their own code.
+
+## PnP Algorithm
+
+This node uses a simple PnP approach to pose estimation from YOLO or AprilTag detections.
+
+## Psuedo-SLAM Node
+
+This node publishes tf2 transforms and analyzes the current images in sight. Combined with dead-reckoning odometry data from the DVL and IMU, it also generates a full global pose over time.
diff --git a/src/guppy_vision/launch/core.xml b/src/guppy_vision/launch/core.xml
index 4bd68b1..aaf9d64 100644
--- a/src/guppy_vision/launch/core.xml
+++ b/src/guppy_vision/launch/core.xml
@@ -1,5 +1,6 @@
 <launch>
     <node pkg="guppy_vision" exec="webcam_pub"/>
+    <node pkg="guppy_vision" exec="camera_pub"/>
     <node pkg="guppy_vision" exec="pnp"/>
     <node pkg="guppy_vision" exec="slam"/>
     <node pkg="guppy_vision" exec="aprilrepub"/>
diff --git a/src/guppy_vision/src/camera_manager.cpp b/src/guppy_vision/src/camera_manager.cpp
new file mode 100644
index 0000000..87c2291
--- /dev/null
+++ b/src/guppy_vision/src/camera_manager.cpp
@@ -0,0 +1,115 @@
+#include "camera_manager.hpp"
+
+using namespace camera_manager;
+
+CameraManager::CameraManager() {
+    system = System::GetInstance();
+    camList = system->GetCameras();
+}
+
+CameraManager::~CameraManager() {
+    cout << "Attempting graceful exit." << endl;
+
+    camList.Clear();
+    system->ReleaseInstance();
+
+    cout << "Successfully exited gracefully." << endl;
+}
+
+void CameraManager::takePicture(CameraPtr pCam) {
+    INodeMap& nodeMapTLDevice = pCam->GetTLDeviceNodeMap();
+    pCam->Init();
+    INodeMap& nodeMap = pCam->GetNodeMap();
+
+    cout << "Acquiring single image from " << nodeMapTLDevice.GetDeviceName() << endl;
+
+    pCam->BeginAcquisition();
+
+    gcstring deviceSerialNumber("");
+    CStringPtr ptrStringSerial = nodeMapTLDevice.GetNode("DeviceSerialNumber");
+    deviceSerialNumber = ptrStringSerial->GetValue();
+
+    ImageProcessor processor;
+    processor.SetColorProcessing(SPINNAKER_COLOR_PROCESSING_ALGORITHM_HQ_LINEAR);
+
+    ImagePtr pResultImage = pCam->GetNextImage(1000);
+
+    const size_t width = pResultImage->GetWidth();
+
+    const size_t height = pResultImage->GetHeight();
+
+    cout << "Grabbed image: width = " << width << ", height = " << height << endl;
+
+    // ImagePtr convertedImage = processor.Convert(pResultImage, PixelFormat_BayerRG8);
+    // convertedImage->Save("saved.png");
+
+    std::cout << pResultImage->GetData() << std::endl;
+
+    pResultImage->Release();
+    pCam->EndAcquisition();
+
+}
+
+int CameraManager::setStreamMode(CameraPtr pCam, StreamMode mode)
+{
+    int result = 0;
+
+    // Retrieve Stream nodemap
+    const INodeMap& sNodeMap = pCam->GetTLStreamNodeMap();
+
+    // The node "StreamMode" is only available for GEV cameras.
+    // Skip setting stream mode if the node is inaccessible.
+    const CEnumerationPtr ptrStreamMode = sNodeMap.GetNode("StreamMode");
+    if (!IsReadable(ptrStreamMode) || !IsWritable(ptrStreamMode))
+    {
+        return 0;
+    }
+
+    gcstring streamMode;
+    switch (mode)
+    {
+        case STREAM_MODE_PGRLWF:
+            streamMode = "LWF";
+            break;
+        case STREAM_MODE_SOCKET:
+            streamMode = "Socket";
+            break;
+        case STREAM_MODE_TELEDYNE_GIGE_VISION:
+        default:
+            streamMode = "TeledyneGigeVision";
+    }
+
+    // Retrieve the desired entry node from the enumeration node
+    const CEnumEntryPtr ptrStreamModeCustom = ptrStreamMode->GetEntryByName(streamMode);
+    if (!IsReadable(ptrStreamModeCustom))
+    {
+        // Failed to get custom node
+        cout << "Stream mode " + streamMode + " not available.  Aborting..." << endl;
+        return -1;
+    }
+    // Retrieve the integer value from the entry node
+    const int64_t streamModeCustom = ptrStreamModeCustom->GetValue();
+
+    // Set integer as new value for enumeration node
+    ptrStreamMode->SetIntValue(streamModeCustom);
+
+    // Print out the current stream mode
+    cout << endl << "Stream Mode set to " + ptrStreamMode->GetCurrentEntry()->GetSymbolic() << "..." << endl;
+
+    return 0;
+}
+
+int main(int argc, char *argv[]) {
+
+    CameraManager manager;
+
+    const unsigned int numCameras = manager.camList.GetSize();
+
+    cout << "Discovered " << numCameras << " cameras." << endl;
+
+    CameraManager::takePicture(manager.camList[0]);
+
+    cout << "Exiting." << endl;
+
+    return 0;
+}
\ No newline at end of file
diff --git a/src/guppy_vision/src/camera_manager.hpp b/src/guppy_vision/src/camera_manager.hpp
new file mode 100644
index 0000000..926d8fb
--- /dev/null
+++ b/src/guppy_vision/src/camera_manager.hpp
@@ -0,0 +1,40 @@
+#ifndef GUPPY_VISION_CAMERA_UTILS_HPP
+#define GUPPY_VISION_CAMERA_UTILS_HPP
+
+#include <Spinnaker.h>
+#include <SpinGenApi/SpinnakerGenApi.h>
+#include <iostream>
+#include <sstream>
+
+using namespace Spinnaker;
+using namespace Spinnaker::GenApi;
+using namespace Spinnaker::GenICam;
+using namespace std;
+
+enum StreamMode
+{
+    STREAM_MODE_TELEDYNE_GIGE_VISION, // windows
+    STREAM_MODE_PGRLWF,               // legacy windows
+    STREAM_MODE_SOCKET, // macos and linux
+};
+
+namespace camera_manager {
+
+    class CameraManager {
+
+    public:
+        CameraManager();
+        ~CameraManager();
+
+        SystemPtr system;
+        CameraList camList;
+
+        static int setStreamMode(CameraPtr pCam, StreamMode mode);
+
+        static void takePicture(CameraPtr pCam);
+
+    };
+
+}
+
+#endif //GUPPY_VISION_CAMERA_UTILS_HPP
\ No newline at end of file
diff --git a/src/guppy_vision/src/camera_pub.cpp b/src/guppy_vision/src/camera_pub.cpp
new file mode 100644
index 0000000..6b5c1b9
--- /dev/null
+++ b/src/guppy_vision/src/camera_pub.cpp
@@ -0,0 +1,99 @@
+#include <CameraDefs.h>
+#include <Spinnaker.h>
+#include <atomic>
+#include <iostream>
+#include <memory>
+#include <string>
+#include <thread>
+#include <vector>
+// #include <SpinGenApi/SpinnakerGenApi.h>
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/fill_image.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "image_transport/image_transport.hpp"
+
+typedef struct camInfo {
+  std::string serial;
+  std::string position;
+} CamInfo;
+
+std::vector<CamInfo> cams = {
+    {.serial = "17473425", .position = "fl"}, {.serial = "17473424", .position = "fr"}, {.serial = "14406636", .position = "d"},  {.serial = "14406334", .position = "rl"},
+    {.serial = "14406637", .position = "rr"}, {.serial = "20188596", .position = "cl"}, {.serial = "20188587", .position = "cr"},
+};
+
+class FlirPublisher : public rclcpp::Node {
+ public:
+  FlirPublisher() : Node("camera_publisher") {
+    this->running_.store(true);
+
+    this->system = Spinnaker::System::GetInstance();
+    this->camList = system->GetCameras();
+
+    std::string base = "/cam/";
+
+    for (unsigned int i = 0; i < camList.GetSize(); ++i) {
+        std::string pos = "unknown";
+      for (CamInfo &c: cams) {
+          if (this->camList[i]->GetDeviceSerialNumber().c_str() == c.serial) {
+              pos = c.position;
+              break;
+          }
+      }
+      std::string topic = "/cam/" + pos + "/raw";
+      this->pubs_.push_back(image_transport::create_publisher(this, topic));
+      // this->publishers_.push_back(this->create_publisher<sensor_msgs::msg::Image>(topic, 1));
+      this->threads_.emplace_back(&FlirPublisher::acquireFootage, this, i);
+    }
+  }
+
+  ~FlirPublisher() {
+    this->running_.store(false);
+
+    for (auto& t : threads_) {
+      if (t.joinable()) {
+        t.join();
+      }
+    }
+
+    this->camList.Clear();
+    this->system->ReleaseInstance();
+  }
+
+ private:
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
+  std::vector<image_transport::Publisher> pubs_;
+
+  Spinnaker::SystemPtr system;
+  Spinnaker::CameraList camList;
+
+  std::thread ackThread;
+  std::atomic<bool> running_{false};
+
+  std::vector<rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr> publishers_;
+  std::vector<std::thread> threads_;
+
+  void acquireFootage(int camIndex) {
+    Spinnaker::CameraPtr pCam = this->camList[camIndex];
+    pCam->Init();
+    pCam->BeginAcquisition();
+    Spinnaker::ImageProcessor processor;
+    while (this->running_) {
+      Spinnaker::ImagePtr pImg = pCam->GetNextImage(1000);
+      auto msg = std::make_unique<sensor_msgs::msg::Image>();
+      sensor_msgs::fillImage(*msg, sensor_msgs::image_encodings::BAYER_RGGB8, pImg->GetHeight(), pImg->GetWidth(), pImg->GetStride(), pImg->GetData());
+      pImg->Release();
+
+      this->pubs_[camIndex].publish(*msg.get());
+      // this->publishers_[camIndex]->publish(std::move(msg));
+    }
+  }
+};
+
+int main(int argc, char* argv[]) {
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<FlirPublisher>());
+  rclcpp::shutdown();
+  return 0;
+}