diff --git a/.clang-format b/.clang-format
new file mode 100644
index 0000000..792fd96
--- /dev/null
+++ b/.clang-format
@@ -0,0 +1,67 @@
+---
+BasedOnStyle: Google
+AccessModifierOffset: -2
+AlignEscapedNewlinesLeft: false
+AlignTrailingComments: true
+AlignAfterOpenBracket: Align
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortFunctionsOnASingleLine: true
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakBeforeMultilineStrings: false
+AlwaysBreakTemplateDeclarations: true
+BinPackArguments: false
+BinPackParameters: false
+BreakBeforeBinaryOperators: false
+BreakBeforeTernaryOperators: false
+BreakConstructorInitializersBeforeComma: true
+ColumnLimit: 120
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 2
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: false
+DerivePointerBinding: false
+ExperimentalAutoDetectBinPacking: false
+IndentCaseLabels: true
+IndentFunctionDeclarationAfterType: false
+IndentWidth: 2
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakBeforeFirstCallParameter: 19
+PenaltyBreakComment: 60
+PenaltyBreakFirstLessLess: 1000
+PenaltyBreakString: 1
+PenaltyExcessCharacter: 1000
+PenaltyReturnTypeOnItsOwnLine: 90
+PointerBindsToType: true
+SortIncludes: false
+SpaceAfterControlStatementKeyword: true
+SpaceAfterCStyleCast: false
+SpaceBeforeAssignmentOperators: true
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 2
+SpacesInAngles: false
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+Standard: Auto
+TabWidth: 2
+UseTab: Never
+
+# Configure each individual brace in BraceWrapping
+BreakBeforeBraces: Custom
+
+# Control of individual brace wrapping cases
+BraceWrapping: {
+    AfterClass: 'true'
+    AfterControlStatement: 'true'
+    AfterEnum : 'true'
+    AfterFunction : 'true'
+    AfterNamespace : 'true'
+    AfterStruct : 'true'
+    AfterUnion : 'true'
+    BeforeCatch : 'true'
+    BeforeElse : 'true'
+    IndentBraces : 'false'
+}
+...
diff --git a/.clang-tidy b/.clang-tidy
new file mode 100644
index 0000000..33c723d
--- /dev/null
+++ b/.clang-tidy
@@ -0,0 +1,80 @@
+---
+Checks: >
+    -*,
+    clang-diagnostic-*,
+    -clang-diagnostic-unknown-warning-option,
+    clang-analyzer-*,
+    -clang-analyzer-cplusplus*,
+    bugprone-*,
+    -bugprone-easily-swappable-parameters,
+    cppcoreguidelines-*,
+    -cppcoreguidelines-macro-usage,
+    -cppcoreguidelines-pro-type-static-cast-downcast,
+    -cppcoreguidelines-pro-type-vararg,
+    -cppcoreguidelines-pro-type-union-access,
+    -cppcoreguidelines-pro-bounds-array-to-pointer-decay,
+    -cppcoreguidelines-pro-bounds-pointer-arithmetic,
+    -cppcoreguidelines-pro-bounds-constant-array-index,
+    -cppcoreguidelines-avoid-magic-numbers,
+    -cppcoreguidelines-non-private-member-variables-in-classes,
+    -cppcoreguidelines-avoid-non-const-global-variables,
+    misc-*,
+    -misc-non-private-member-variables-in-classes,
+    -misc-no-recursion,
+    modernize-*,
+    -modernize-use-trailing-return-type,
+    -modernize-use-nodiscard,
+    performance-*,
+    readability-*,
+    -readability-braces-around-statements,
+    -readability-named-parameter,
+    -readability-magic-numbers,
+    -readability-isolate-declaration,
+    -readability-identifier-length,
+    -readability-function-cognitive-complexity
+WarningsAsErrors: >
+    -*,
+    clang-diagnostic-*,
+    -clang-diagnostic-unknown-warning-option,
+    clang-analyzer-*,
+    -clang-analyzer-cplusplus*,
+    bugprone-*,
+    -bugprone-easily-swappable-parameters,
+    cppcoreguidelines-*,
+    -cppcoreguidelines-macro-usage,
+    -cppcoreguidelines-pro-type-static-cast-downcast,
+    -cppcoreguidelines-pro-type-vararg,
+    -cppcoreguidelines-pro-type-union-access,
+    -cppcoreguidelines-pro-bounds-array-to-pointer-decay,
+    -cppcoreguidelines-pro-bounds-pointer-arithmetic,
+    -cppcoreguidelines-pro-bounds-constant-array-index,
+    -cppcoreguidelines-avoid-magic-numbers,
+    -cppcoreguidelines-non-private-member-variables-in-classes,
+    -cppcoreguidelines-avoid-non-const-global-variables,
+    misc-*,
+    -misc-non-private-member-variables-in-classes,
+    -misc-no-recursion,
+    modernize-*,
+    -modernize-use-trailing-return-type,
+    -modernize-use-nodiscard,
+    performance-*,
+    readability-*,
+    -readability-braces-around-statements,
+    -readability-named-parameter,
+    -readability-magic-numbers,
+    -readability-isolate-declaration,
+    -readability-identifier-length,
+    -readability-function-cognitive-complexity
+HeaderFilterRegex: '.*'
+AnalyzeTemporaryDtors: false
+FormatStyle:     none
+CheckOptions:
+  - key:             modernize-use-override.AllowOverrideAndFinal
+    value:           '1'
+  - key:             cppcoreguidelines-explicit-virtual-functions.AllowOverrideAndFinal
+    value:           '1'
+  - key:             cppcoreguidelines-special-member-functions.AllowSoleDefaultDtor
+    value:           '1'
+ExtraArgs:
+  - '-std=c++17'
+...
diff --git a/.cmake-format b/.cmake-format
new file mode 100644
index 0000000..b162026
--- /dev/null
+++ b/.cmake-format
@@ -0,0 +1,334 @@
+#!/bin/python
+# ----------------------------------
+# Options affecting listfile parsing
+# ----------------------------------
+with section("parse"):
+
+  # Specify structure for custom cmake functions
+  additional_commands = {
+    'target_clang_tidy': {
+      'pargs': {'nargs': 1},
+      'kwargs': {
+        'ARGUMENTS': '*',
+        'ENABLE': 1,
+      }
+    },
+    'target_include_what_you_use': {
+      'pargs': {'nargs': 1},
+      'kwargs': {
+        'ARGUMENTS': '*',
+        'ENABLE': 1,
+      }
+    },
+    'include_what_you_use': {
+      'pargs': {'nargs': 0},
+      'kwargs': {
+        'ARGUMENTS': '*',
+        'ENABLE': 1,
+      }
+    },
+    'target_cppcheck': {
+      'pargs': {'nargs': 1},
+      'kwargs': {
+        'ARGUMENTS': '*',
+        'ENABLE': 1,
+      }
+    },
+    'cppcheck': {
+      'pargs': {'nargs': 0},
+      'kwargs': {
+        'ARGUMENTS': '*',
+        'ENABLE': 1,
+      }
+    },
+    'configure_package': {
+      'pargs': {'nargs': 0},
+      'kwargs': {
+        'NAMESPACE': '?',
+        'TARGETS': '*',
+        'DEPENDENCIES': '*',
+      }
+    },
+    'add_gtest_discover_tests': {
+      'pargs': {'nargs': 1},
+    },
+    'add_run_tests_target': {
+      'pargs': {'nargs': 0},
+      'kwargs': {
+        'ENABLE': 1,
+      }
+    },
+    'add_run_benchmark_target': {
+      'pargs': {'nargs': 0},
+      'kwargs': {
+        'ENABLE': 1,
+      }
+    },
+    'target_cxx_version': {
+      'pargs': {'nargs': 1},
+      'kwargs': {
+        'INTERFACE': '+',
+        'PUBLIC': '+',
+        'PRIVATE': '+',
+        'VERSION': 1,
+      }
+    },
+    'target_code_coverage': {
+      'pargs': {'nargs': 1},
+      'kwargs': {
+        'AUTO': '+',
+        'ALL': '+',
+        'EXTERNAL': '+',
+        'PRIVATE': '+',
+        'PUBLIC': '+',
+        'INTERFACE': '+',
+        'ENABLE': 1,
+        'EXCLUDE': '*',
+      }
+    },
+    'add_code_coverage': {
+      'pargs': {'nargs': 0},
+      'kwargs': {
+        'ENABLE': 1,
+      }
+    },
+    'add_code_coverage_all_targets': {
+      'pargs': {'nargs': 0},
+      'kwargs': {
+        'ENABLE': 1,
+        'EXCLUDE': '*',
+      }
+    },
+  }
+
+  # Override configurations per-command where available
+  override_spec = {}
+
+  # Specify variable tags.
+  vartags = []
+
+  # Specify property tags.
+  proptags = []
+
+# -----------------------------
+# Options affecting formatting.
+# -----------------------------
+with section("format"):
+
+  # Disable formatting entirely, making cmake-format a no-op
+  disable = False
+
+  # How wide to allow formatted cmake files
+  line_width = 120
+
+  # How many spaces to tab for indent
+  tab_size = 2
+
+  # If true, lines are indented using tab characters (utf-8 0x09) instead of
+  # <tab_size> space characters (utf-8 0x20). In cases where the layout would
+  # require a fractional tab character, the behavior of the  fractional
+  # indentation is governed by <fractional_tab_policy>
+  use_tabchars = False
+
+  # If <use_tabchars> is True, then the value of this variable indicates how
+  # fractional indentions are handled during whitespace replacement. If set to
+  # 'use-space', fractional indentation is left as spaces (utf-8 0x20). If set
+  # to `round-up` fractional indentation is replaced with a single tab character
+  # (utf-8 0x09) effectively shifting the column to the next tabstop
+  fractional_tab_policy = 'use-space'
+
+  # If an argument group contains more than this many sub-groups (parg or kwarg
+  # groups) then force it to a vertical layout.
+  max_subgroups_hwrap = 2
+
+  # If a positional argument group contains more than this many arguments, then
+  # force it to a vertical layout.
+  max_pargs_hwrap = 3
+
+  # If a cmdline positional group consumes more than this many lines without
+  # nesting, then invalidate the layout (and nest)
+  max_rows_cmdline = 2
+
+  # If true, separate flow control names from their parentheses with a space
+  separate_ctrl_name_with_space = False
+
+  # If true, separate function names from parentheses with a space
+  separate_fn_name_with_space = False
+
+  # If a statement is wrapped to more than one line, than dangle the closing
+  # parenthesis on its own line.
+  dangle_parens = True
+
+  # If the trailing parenthesis must be 'dangled' on its on line, then align it
+  # to this reference: `prefix`: the start of the statement,  `prefix-indent`:
+  # the start of the statement, plus one indentation  level, `child`: align to
+  # the column of the arguments
+  dangle_align = 'prefix'
+
+  # If the statement spelling length (including space and parenthesis) is
+  # smaller than this amount, then force reject nested layouts.
+  min_prefix_chars = 4
+
+  # If the statement spelling length (including space and parenthesis) is larger
+  # than the tab width by more than this amount, then force reject un-nested
+  # layouts.
+  max_prefix_chars = 10
+
+  # If a candidate layout is wrapped horizontally but it exceeds this many
+  # lines, then reject the layout.
+  max_lines_hwrap = 2
+
+  # What style line endings to use in the output.
+  line_ending = 'unix'
+
+  # Format command names consistently as 'lower' or 'upper' case
+  command_case = 'canonical'
+
+  # Format keywords consistently as 'lower' or 'upper' case
+  keyword_case = 'unchanged'
+
+  # A list of command names which should always be wrapped
+  always_wrap = []
+
+  # If true, the argument lists which are known to be sortable will be sorted
+  # lexicographicall
+  enable_sort = True
+
+  # If true, the parsers may infer whether or not an argument list is sortable
+  # (without annotation).
+  autosort = False
+
+  # By default, if cmake-format cannot successfully fit everything into the
+  # desired linewidth it will apply the last, most agressive attempt that it
+  # made. If this flag is True, however, cmake-format will print error, exit
+  # with non-zero status code, and write-out nothing
+  require_valid_layout = False
+
+  # A dictionary mapping layout nodes to a list of wrap decisions. See the
+  # documentation for more information.
+  layout_passes = {}
+
+# ------------------------------------------------
+# Options affecting comment reflow and formatting.
+# ------------------------------------------------
+with section("markup"):
+
+  # What character to use for bulleted lists
+  bullet_char = '*'
+
+  # What character to use as punctuation after numerals in an enumerated list
+  enum_char = '.'
+
+  # If comment markup is enabled, don't reflow the first comment block in each
+  # listfile. Use this to preserve formatting of your copyright/license
+  # statements.
+  first_comment_is_literal = False
+
+  # If comment markup is enabled, don't reflow any comment block which matches
+  # this (regex) pattern. Default is `None` (disabled).
+  literal_comment_pattern = None
+
+  # Regular expression to match preformat fences in comments default=
+  # ``r'^\s*([`~]{3}[`~]*)(.*)$'``
+  fence_pattern = '^\\s*([`~]{3}[`~]*)(.*)$'
+
+  # Regular expression to match rulers in comments default=
+  # ``r'^\s*[^\w\s]{3}.*[^\w\s]{3}$'``
+  ruler_pattern = '^\\s*[^\\w\\s]{3}.*[^\\w\\s]{3}$'
+
+  # If a comment line matches starts with this pattern then it is explicitly a
+  # trailing comment for the preceeding argument. Default is '#<'
+  explicit_trailing_pattern = '#<'
+
+  # If a comment line starts with at least this many consecutive hash
+  # characters, then don't lstrip() them off. This allows for lazy hash rulers
+  # where the first hash char is not separated by space
+  hashruler_min_length = 10
+
+  # If true, then insert a space between the first hash char and remaining hash
+  # chars in a hash ruler, and normalize its length to fill the column
+  canonicalize_hashrulers = True
+
+  # enable comment markup parsing and reflow
+  enable_markup = True
+
+# ----------------------------
+# Options affecting the linter
+# ----------------------------
+with section("lint"):
+
+  # a list of lint codes to disable
+  disabled_codes = []
+
+  # regular expression pattern describing valid function names
+  function_pattern = '[0-9a-z_]+'
+
+  # regular expression pattern describing valid macro names
+  macro_pattern = '[0-9A-Z_]+'
+
+  # regular expression pattern describing valid names for variables with global
+  # (cache) scope
+  global_var_pattern = '[A-Z][0-9A-Z_]+'
+
+  # regular expression pattern describing valid names for variables with global
+  # scope (but internal semantic)
+  internal_var_pattern = '_[A-Z][0-9A-Z_]+'
+
+  # regular expression pattern describing valid names for variables with local
+  # scope
+  local_var_pattern = '[a-z][a-z0-9_]+'
+
+  # regular expression pattern describing valid names for privatedirectory
+  # variables
+  private_var_pattern = '_[0-9a-z_]+'
+
+  # regular expression pattern describing valid names for public directory
+  # variables
+  public_var_pattern = '[A-Z][0-9A-Z_]+'
+
+  # regular expression pattern describing valid names for function/macro
+  # arguments and loop variables.
+  argument_var_pattern = '[a-z][a-z0-9_]+'
+
+  # regular expression pattern describing valid names for keywords used in
+  # functions or macros
+  keyword_pattern = '[A-Z][0-9A-Z_]+'
+
+  # In the heuristic for C0201, how many conditionals to match within a loop in
+  # before considering the loop a parser.
+  max_conditionals_custom_parser = 2
+
+  # Require at least this many newlines between statements
+  min_statement_spacing = 1
+
+  # Require no more than this many newlines between statements
+  max_statement_spacing = 2
+  max_returns = 6
+  max_branches = 12
+  max_arguments = 5
+  max_localvars = 15
+  max_statements = 50
+
+# -------------------------------
+# Options affecting file encoding
+# -------------------------------
+with section("encode"):
+
+  # If true, emit the unicode byte-order mark (BOM) at the start of the file
+  emit_byteorder_mark = False
+
+  # Specify the encoding of the input file. Defaults to utf-8
+  input_encoding = 'utf-8'
+
+  # Specify the encoding of the output file. Defaults to utf-8. Note that cmake
+  # only claims to support utf-8 so be careful when using anything else
+  output_encoding = 'utf-8'
+
+# -------------------------------------
+# Miscellaneous configurations options.
+# -------------------------------------
+with section("misc"):
+
+  # A dictionary containing any per-command configuration overrides. Currently
+  # only `command_case` is supported.
+  per_command = {}
diff --git a/.github/workflows/clang_format.yml b/.github/workflows/clang_format.yml
new file mode 100644
index 0000000..fb4a3f9
--- /dev/null
+++ b/.github/workflows/clang_format.yml
@@ -0,0 +1,30 @@
+name: Clang-Format
+
+on:
+  push:
+    branches:
+      - main
+  pull_request:
+    paths:
+      - 'include'
+      - 'src'
+      - '.github/workflows/clang_format.yml'
+      - '**clang-format'
+  schedule:
+    - cron: '0 5 * * *'
+
+jobs:
+  clang_format:
+    runs-on: ubuntu-22.04
+    steps:
+      - uses: actions/checkout@v4
+
+      - name: Run clang format
+        run: |
+          sudo apt update
+          sudo apt install -y git clang-format-14
+          if [ $? -ge 1 ]; then return 1; fi
+          ./.run-clang-format
+          if [ $? -ge 1 ]; then return 1; fi
+          output=$(git diff)
+          if [ -n "$output" ]; then exit 1; else exit 0; fi
diff --git a/.github/workflows/cmake_format.yml b/.github/workflows/cmake_format.yml
new file mode 100644
index 0000000..933131c
--- /dev/null
+++ b/.github/workflows/cmake_format.yml
@@ -0,0 +1,33 @@
+name: CMake-Format
+
+on:
+  push:
+    branches:
+      - main
+  pull_request:
+    paths:
+      - 'CMakeLists.txt'
+      - 'test/CMakeLists.txt'
+      - '.github/workflows/cmake_format.yml'
+      - '**cmake-format'
+  schedule:
+    - cron: '0 5 * * *'
+
+jobs:
+  cmake_format:
+    runs-on: ubuntu-22.04
+    steps:
+      - uses: actions/checkout@v1
+
+      - name: Run CMake Lang Format Check
+        run: |
+          sudo pip3 install cmakelang
+          RED='\033[0;31m'
+          NC='\033[0m' # No Color
+          git config --global --add safe.directory '*'
+          ./.run-cmake-format
+          output=$(git diff)
+          if [ -n "$output" ]; then printf "${RED}CMake format error: run script './.run-cmake-formate'${NC}\n"; fi
+          if [ -n "$output" ]; then printf "${RED}${output}${NC}\n"; fi
+          if [ -n "$output" ]; then exit 1; else exit 0; fi
+
diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml
new file mode 100644
index 0000000..64eda22
--- /dev/null
+++ b/.github/workflows/docker.yml
@@ -0,0 +1,61 @@
+name: Docker
+
+on:
+  push:
+    branches:
+      - main
+  pull_request:
+  release:
+    types:
+      - released
+
+jobs:
+  ci:
+    name: ${{ matrix.distro }}
+    runs-on: ubuntu-latest
+    strategy:
+      fail-fast: false
+      matrix:
+        distro: [humble, jazzy]
+    env:
+      REGISTRY: ghcr.io
+      IMAGE_NAME: ${{ github.repository }}
+      PUSH_DOCKER_IMAGE: ${{ github.ref == 'refs/heads/master' || github.event_name == 'release' }}
+    permissions:
+      contents: read
+      packages: write
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v4
+
+      - name: Log in to the Container registry
+        uses: docker/login-action@v3
+        with:
+          registry: ${{ env.REGISTRY }}
+          username: ${{ github.actor }}
+          password: ${{ secrets.GITHUB_TOKEN }}
+
+      - name: Extract Docker meta-information
+        id: meta
+        uses: docker/metadata-action@v5
+        with:
+          images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }}
+          flavor: |
+            latest=false
+            prefix=
+            suffix=
+          tags: |
+            type=ref,event=branch,prefix=${{ matrix.distro }}-
+            type=ref,event=pr,prefix=${{ matrix.distro }}-
+            type=semver,pattern={{major}}.{{minor}},prefix=${{ matrix.distro }}-
+
+      - name: Build and push Docker image
+        uses: docker/build-push-action@v5
+        with:
+          context: .
+          file: docker/Dockerfile
+          build-args: |
+            DISTRO=${{ matrix.distro }}
+          push: ${{ env.PUSH_DOCKER_IMAGE }}
+          tags: ${{ steps.meta.outputs.tags }}
+          labels: ${{ steps.meta.outputs.labels }}
diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml
new file mode 100644
index 0000000..4704fb7
--- /dev/null
+++ b/.github/workflows/ubuntu.yml
@@ -0,0 +1,44 @@
+name: ROS [Humble, Jazzy]
+
+on:
+  push:
+    branches:
+      - main
+  pull_request:
+  schedule:
+    - cron: '0 5 * * *'
+  workflow_dispatch:
+  release:
+    types:
+      - released
+
+jobs:
+  ci:
+    name: ${{ matrix.distro }}
+    runs-on: ubuntu-latest
+    strategy:
+      fail-fast: false
+      matrix:
+        distro: [humble, jazzy]
+    container:
+      image: ros:${{ matrix.distro }}
+      env:
+        CCACHE_DIR: ${{ github.workspace }}/${{ matrix.distro }}/.ccache
+        DEBIAN_FRONTEND: noninteractive
+        TZ: Etc/UTC
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v4
+        with:
+          path: target_ws/src
+
+      - name: Build and Tests
+        uses: tesseract-robotics/colcon-action@v11
+        with:
+          ccache-prefix: ${{ matrix.distro }}
+          add-ros-ppa: false
+          vcs-file: dependencies.repos
+          rosdep-install-args: '-iry --skip-keys libvtk'
+          before-script: source /opt/ros/${{ matrix.distro }}/setup.bash
+          target-path: target_ws/src
+          target-args: --cmake-args -DCMAKE_BUILD_TYPE=Debug -DENABLE_TESTING=ON
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..bee8a64
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+__pycache__
diff --git a/.run-clang-format b/.run-clang-format
new file mode 100755
index 0000000..15bd568
--- /dev/null
+++ b/.run-clang-format
@@ -0,0 +1,2 @@
+#!/bin/bash
+find . -type f -regex '.*\.\(cpp\|hpp\|cc\|cxx\|h\|hxx\)' -exec clang-format-14 -style=file -i {} \;
diff --git a/.run-cmake-format b/.run-cmake-format
new file mode 100755
index 0000000..fe66f6d
--- /dev/null
+++ b/.run-cmake-format
@@ -0,0 +1,2 @@
+#!/bin/bash
+find . \( -name CMakeLists.txt -o -name \*.cmake \) -exec cmake-format -i {} \;
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..030e965
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,72 @@
+cmake_minimum_required(VERSION 3.5)
+project(noether_ros)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(noether_tpp REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(yaml-cpp REQUIRED)
+
+# Generate the messages
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(
+  ${PROJECT_NAME}
+  "msg/ToolPath.msg"
+  "msg/ToolPaths.msg"
+  "srv/PlanToolPath.srv"
+  DEPENDENCIES
+  builtin_interfaces
+  geometry_msgs
+)
+rosidl_get_typesupport_target(interfaces_target "${PROJECT_NAME}" "rosidl_typesupport_cpp")
+
+# Library
+add_library(${PROJECT_NAME}_conversions SHARED src/conversions.cpp)
+target_include_directories(
+  ${PROJECT_NAME}_conversions PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+                                     "$<INSTALL_INTERFACE:include>"
+)
+target_link_libraries(${PROJECT_NAME}_conversions noether::noether_tpp ${interfaces_target})
+ament_target_dependencies(${PROJECT_NAME}_conversions tf2_eigen)
+
+# Tool path planning server
+add_executable(${PROJECT_NAME}_tool_path_planning_server src/tool_path_planning_server.cpp)
+target_link_libraries(${PROJECT_NAME}_tool_path_planning_server ${PROJECT_NAME}_conversions yaml-cpp)
+ament_target_dependencies(${PROJECT_NAME}_tool_path_planning_server rclcpp)
+
+if(${ENABLE_TESTING})
+  enable_testing()
+  add_subdirectory(test)
+endif()
+
+# Install the headers
+install(DIRECTORY include/ DESTINATION include/)
+
+# Install the library
+install(
+  TARGETS ${PROJECT_NAME}_conversions
+  EXPORT ${PROJECT_NAME}-targets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+ament_export_targets(${PROJECT_NAME}-targets HAS_LIBRARY_TARGET)
+
+# Install the executable
+install(TARGETS ${PROJECT_NAME}_tool_path_planning_server DESTINATION lib/${PROJECT_NAME})
+
+ament_export_dependencies(
+  builtin_interfaces
+  geometry_msgs
+  rosidl_default_runtime
+  tf2_eigen
+)
+ament_package()
diff --git a/ci b/ci
new file mode 120000
index 0000000..ecd623e
--- /dev/null
+++ b/ci
@@ -0,0 +1 @@
+.github/workflows/
\ No newline at end of file
diff --git a/dependencies.repos b/dependencies.repos
new file mode 100644
index 0000000..d9b5f48
--- /dev/null
+++ b/dependencies.repos
@@ -0,0 +1,12 @@
+- git:
+    local-name: ros_industrial_cmake_boilerplate
+    uri: https://github.com/ros-industrial/ros_industrial_cmake_boilerplate.git
+    version: 0.7.4
+- git:
+    local-name: boost_plugin_loader
+    uri: https://github.com/tesseract-robotics/boost_plugin_loader.git
+    version: 0.3.2
+- git:
+    local-name: noether
+    uri: https://github.com/ros-industrial/noether.git
+    version: cca58d61103174394c31e7f9d85e0e887f879390
diff --git a/docker/Dockerfile b/docker/Dockerfile
new file mode 100644
index 0000000..863be7b
--- /dev/null
+++ b/docker/Dockerfile
@@ -0,0 +1,41 @@
+ARG DISTRO=humble
+FROM ros:${DISTRO}
+ARG DISTRO
+
+SHELL ["/bin/bash", "-c"]
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+USER root
+
+# Install
+RUN apt update \
+  && apt upgrade -y \
+  && apt install -y cmake curl git python3
+
+# Install and configure ROS infrastructure tools
+RUN apt update \
+  && apt install -y python3-vcstool python3-colcon-common-extensions python3-rosdep \
+  && rosdep update
+
+# Install the dependency repositories
+# Use a tmpfs mount for the workspace so as not to unnecessarily copy files into the final image
+# Bind mount the source directory so as not to unnecessarily copy source code into the docker image
+ARG WORKSPACE_DIR=/tmpfs/noether_ros
+ARG INSTALL_DIR=/opt/noether_ros
+RUN mkdir -p ${INSTALL_DIR}
+
+RUN --mount=type=tmpfs,target=${WORKSPACE_DIR} --mount=type=bind,target=${WORKSPACE_DIR}/src/noether_ros \
+  source /opt/ros/${DISTRO}/setup.bash \
+  && cd ${WORKSPACE_DIR} \
+  && vcs import src < src/noether_ros/dependencies.repos --shallow \
+  && rosdep install \
+    --from-paths ${WORKSPACE_DIR}/src \
+    -iry \
+    --skip-keys libvtk \
+  && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release \
+  && cp -r ${WORKSPACE_DIR}/install ${INSTALL_DIR}
+
+# Set the entrypoint to source the workspace
+COPY docker/entrypoint.sh /entrypoint.sh
+ENTRYPOINT ["/entrypoint.sh"]
diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml
new file mode 100644
index 0000000..4935e53
--- /dev/null
+++ b/docker/docker-compose.yml
@@ -0,0 +1,18 @@
+services:
+  noether:
+    build:
+      context: ..
+      dockerfile: docker/Dockerfile
+      args:
+        DISTRO: humble
+    environment:
+      ROS_LOG_DIR: /tmp
+      ROS_LOCALHOST_ONLY: $ROS_LOCALHOST_ONLY
+      ROS_DOMAIN_ID: $ROS_DOMAIN_ID
+    container_name: noether_ros
+    image: ghcr.io/ros-industrial/noether_ros:humble
+    stdin_open: true
+    tty: true
+    network_mode: host
+    ipc: host
+    privileged: false
diff --git a/docker/entrypoint.sh b/docker/entrypoint.sh
new file mode 100755
index 0000000..bea94a9
--- /dev/null
+++ b/docker/entrypoint.sh
@@ -0,0 +1,3 @@
+#! /bin/bash
+source /opt/noether_ros/install/setup.bash
+ros2 run noether_ros noether_ros_tool_path_planning_server
diff --git a/include/noether_ros/conversions.h b/include/noether_ros/conversions.h
new file mode 100644
index 0000000..c8a026e
--- /dev/null
+++ b/include/noether_ros/conversions.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <noether_ros/msg/tool_paths.hpp>
+#include <noether_tpp/core/types.h>
+
+namespace noether_ros
+{
+geometry_msgs::msg::PoseArray toMsg(const noether::ToolPathSegment& segment, const std::string frame = "");
+noether::ToolPathSegment toMsg(const geometry_msgs::msg::PoseArray& segment_msg);
+
+noether_ros::msg::ToolPath toMsg(const noether::ToolPath& tool_path, const std::string frame = "");
+noether::ToolPath fromMsg(const noether_ros::msg::ToolPath& tool_path_msg);
+
+noether_ros::msg::ToolPaths toMsg(const noether::ToolPaths& tool_paths, const std::string frame = "");
+noether::ToolPaths fromMsg(const noether_ros::msg::ToolPaths& tool_paths_msg);
+
+std::vector<noether_ros::msg::ToolPaths> toMsg(const std::vector<noether::ToolPaths>& tool_paths_list,
+                                               const std::string& frame = "");
+std::vector<noether::ToolPaths> fromMsg(const std::vector<noether_ros::msg::ToolPaths>& tool_paths_msg);
+
+geometry_msgs::msg::PoseArray flattenToMsg(const noether::ToolPath& tool_paths, const std::string frame = "");
+geometry_msgs::msg::PoseArray flattenToMsg(const noether::ToolPaths& tool_paths, const std::string frame = "");
+geometry_msgs::msg::PoseArray flattenToMsg(const std::vector<noether::ToolPaths>& tool_paths,
+                                           const std::string frame = "");
+
+}  // namespace noether_ros
diff --git a/msg/ToolPath.msg b/msg/ToolPath.msg
new file mode 100644
index 0000000..3fbf980
--- /dev/null
+++ b/msg/ToolPath.msg
@@ -0,0 +1 @@
+geometry_msgs/PoseArray[] segments
diff --git a/msg/ToolPaths.msg b/msg/ToolPaths.msg
new file mode 100644
index 0000000..cfa7908
--- /dev/null
+++ b/msg/ToolPaths.msg
@@ -0,0 +1 @@
+noether_ros/ToolPath[] tool_paths
diff --git a/package.xml b/package.xml
new file mode 100644
index 0000000..d6b39fc
--- /dev/null
+++ b/package.xml
@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>noether_ros</name>
+  <version>0.0.0</version>
+  <description>ROS2 integration for Noether</description>
+  <author email="michael.ripperger@swri.org">Michael Ripperger</author>
+  <maintainer email="michael.ripperger@swri.org">Michael Ripperger</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <depend>builtin_interfaces</depend>
+  <depend>geometry_msgs</depend>
+  <depend>noether_tpp</depend>
+  <depend>rclcpp</depend>
+  <depend>tf2_eigen</depend>
+  <depend>yaml-cpp</depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_index_python</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>launch_ros</test_depend>
+  <test_depend>launch_testing</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>rclpy</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/conversions.cpp b/src/conversions.cpp
new file mode 100644
index 0000000..754cc3e
--- /dev/null
+++ b/src/conversions.cpp
@@ -0,0 +1,157 @@
+#include <noether_ros/conversions.h>
+
+#include <tf2_eigen/tf2_eigen.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
+
+namespace noether_ros
+{
+geometry_msgs::msg::PoseArray toMsg(const noether::ToolPathSegment& segment, const std::string frame)
+{
+  geometry_msgs::msg::PoseArray segment_msg;
+  segment_msg.header.frame_id = frame;
+  segment_msg.poses.reserve(segment.size());
+
+  std::transform(segment.begin(),
+                 segment.end(),
+                 std::back_inserter(segment_msg.poses),
+                 [](const Eigen::Isometry3d& pose) { return tf2::toMsg(pose); });
+
+  return segment_msg;
+}
+
+noether::ToolPathSegment fromMsg(const geometry_msgs::msg::PoseArray& segment_msg)
+{
+  noether::ToolPathSegment segment;
+  segment.reserve(segment_msg.poses.size());
+
+  std::transform(segment_msg.poses.begin(),
+                 segment_msg.poses.end(),
+                 std::back_inserter(segment),
+                 [](const geometry_msgs::msg::Pose& msg) {
+                   Eigen::Isometry3d pose;
+                   tf2::fromMsg(msg, pose);
+                   return pose;
+                 });
+
+  return segment;
+}
+
+noether_ros::msg::ToolPath toMsg(const noether::ToolPath& tool_path, const std::string frame)
+{
+  noether_ros::msg::ToolPath tool_path_msg;
+  tool_path_msg.segments.reserve(tool_path.size());
+
+  std::transform(tool_path.begin(),
+                 tool_path.end(),
+                 std::back_inserter(tool_path_msg.segments),
+                 [&frame](const noether::ToolPathSegment& segment) { return toMsg(segment, frame); });
+
+  return tool_path_msg;
+}
+
+noether::ToolPath fromMsg(const noether_ros::msg::ToolPath& tool_path_msg)
+{
+  noether::ToolPath tool_path;
+  tool_path.reserve(tool_path_msg.segments.size());
+
+  std::transform(tool_path_msg.segments.begin(),
+                 tool_path_msg.segments.end(),
+                 std::back_inserter(tool_path),
+                 [](const geometry_msgs::msg::PoseArray& msg) { return fromMsg(msg); });
+
+  return tool_path;
+}
+
+noether_ros::msg::ToolPaths toMsg(const noether::ToolPaths& tool_paths, const std::string frame)
+{
+  noether_ros::msg::ToolPaths tool_paths_msg;
+  tool_paths_msg.tool_paths.reserve(tool_paths.size());
+
+  std::transform(tool_paths.begin(),
+                 tool_paths.end(),
+                 std::back_inserter(tool_paths_msg.tool_paths),
+                 [&frame](const noether::ToolPath& tool_path) { return toMsg(tool_path, frame); });
+
+  return tool_paths_msg;
+}
+
+noether::ToolPaths fromMsg(const noether_ros::msg::ToolPaths& tool_paths_msg)
+{
+  noether::ToolPaths tool_paths;
+  tool_paths.reserve(tool_paths_msg.tool_paths.size());
+
+  std::transform(tool_paths_msg.tool_paths.begin(),
+                 tool_paths_msg.tool_paths.end(),
+                 std::back_inserter(tool_paths),
+                 [](const noether_ros::msg::ToolPath& msg) { return fromMsg(msg); });
+
+  return tool_paths;
+}
+
+std::vector<noether_ros::msg::ToolPaths> toMsg(const std::vector<noether::ToolPaths>& tool_paths_list,
+                                               const std::string& frame)
+{
+  std::vector<noether_ros::msg::ToolPaths> tool_paths_list_msg;
+  tool_paths_list_msg.reserve(tool_paths_list.size());
+  std::transform(tool_paths_list.begin(),
+                 tool_paths_list.end(),
+                 std::back_inserter(tool_paths_list_msg),
+                 [&frame](const noether::ToolPaths& tp) { return noether_ros::toMsg(tp, frame); });
+
+  return tool_paths_list_msg;
+}
+
+std::vector<noether::ToolPaths> fromMsg(const std::vector<noether_ros::msg::ToolPaths>& tool_paths_list_msg)
+{
+  std::vector<noether::ToolPaths> tool_paths_list;
+  tool_paths_list.reserve(tool_paths_list_msg.size());
+  std::transform(
+      tool_paths_list_msg.begin(),
+      tool_paths_list_msg.end(),
+      std::back_inserter(tool_paths_list),
+      [](const noether_ros::msg::ToolPaths& tool_paths_msg) { return noether_ros::fromMsg(tool_paths_msg); });
+
+  return tool_paths_list;
+}
+
+geometry_msgs::msg::PoseArray flattenToMsg(const noether::ToolPath& tool_path, const std::string frame)
+{
+  geometry_msgs::msg::PoseArray msg;
+  msg.header.frame_id = frame;
+
+  for (const noether::ToolPathSegment& segment : tool_path)
+    for (const Eigen::Isometry3d& pose : segment)
+      msg.poses.push_back(tf2::toMsg(pose));
+
+  return msg;
+}
+
+geometry_msgs::msg::PoseArray flattenToMsg(const noether::ToolPaths& tool_paths, const std::string frame)
+{
+  geometry_msgs::msg::PoseArray msg;
+  msg.header.frame_id = frame;
+
+  for (const noether::ToolPath& tool_path : tool_paths)
+    for (const noether::ToolPathSegment& segment : tool_path)
+      for (const Eigen::Isometry3d& pose : segment)
+        msg.poses.push_back(tf2::toMsg(pose));
+
+  return msg;
+}
+
+geometry_msgs::msg::PoseArray flattenToMsg(const std::vector<noether::ToolPaths>& tool_paths_list,
+                                           const std::string frame)
+{
+  geometry_msgs::msg::PoseArray msg;
+  msg.header.frame_id = frame;
+
+  for (const noether::ToolPaths& tool_paths : tool_paths_list)
+    for (const noether::ToolPath& tool_path : tool_paths)
+      for (const noether::ToolPathSegment& segment : tool_path)
+        for (const Eigen::Isometry3d& pose : segment)
+          msg.poses.push_back(tf2::toMsg(pose));
+
+  return msg;
+}
+
+}  // namespace noether_ros
diff --git a/src/tool_path_planning_server.cpp b/src/tool_path_planning_server.cpp
new file mode 100644
index 0000000..0f47e26
--- /dev/null
+++ b/src/tool_path_planning_server.cpp
@@ -0,0 +1,76 @@
+#include <noether_ros/conversions.h>
+#include <noether_ros/srv/plan_tool_path.hpp>
+
+#include <noether_tpp/core/tool_path_planner_pipeline.h>
+#include <noether_tpp/utils.h>
+#include <noether_tpp/plugin_interface.h>
+#include <pcl/io/vtk_lib_io.h>
+#include <rclcpp/rclcpp.hpp>
+#include <yaml-cpp/yaml.h>
+
+class ToolPathPlanningServer : public rclcpp::Node
+{
+public:
+  ToolPathPlanningServer() : Node("tool_path_planning_server")
+  {
+    server_ = this->create_service<noether_ros::srv::PlanToolPath>(
+        "plan_tool_path",
+        std::bind(&ToolPathPlanningServer::callback, this, std::placeholders::_1, std::placeholders::_2));
+  }
+
+protected:
+  void callback(const std::shared_ptr<noether_ros::srv::PlanToolPath::Request> request,
+                std::shared_ptr<noether_ros::srv::PlanToolPath::Response> response)
+  {
+    try
+    {
+      // Convert input configuration string/file to YAML node
+      YAML::Node config;
+      try
+      {
+        config = YAML::LoadFile(request->config);
+      }
+      catch (const YAML::Exception&)
+      {
+        config = YAML::Load(request->config);
+      }
+
+      // Load the mesh file
+      pcl::PolygonMesh mesh;
+
+      // Set the mesh frame from the request
+      mesh.header.frame_id = request->mesh_frame;
+
+      if (pcl::io::loadPolygonFile(request->mesh_file, mesh) <= 0)
+        throw std::runtime_error("Failed to load mesh from file: " + request->mesh_file);
+
+      // Create and run the tool path planning pipeline
+      noether::Factory factory;
+      noether::ToolPathPlannerPipeline pipeline(factory, config);
+      std::vector<noether::ToolPaths> tool_paths = pipeline.plan(mesh);
+
+      // Fill in the response
+      response->success = true;
+      response->tool_paths = noether_ros::toMsg(tool_paths, request->mesh_frame);
+    }
+    catch (const std::exception& ex)
+    {
+      response->success = false;
+      std::stringstream ss;
+      noether::printException(ex, ss);
+      response->message = ss.str();
+    }
+  }
+
+  rclcpp::Service<noether_ros::srv::PlanToolPath>::SharedPtr server_;
+};
+
+int main(int argc, char* argv[])
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<ToolPathPlanningServer>();
+  RCLCPP_INFO_STREAM(node->get_logger(), "Started tool path planning server");
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}
diff --git a/srv/PlanToolPath.srv b/srv/PlanToolPath.srv
new file mode 100644
index 0000000..893689c
--- /dev/null
+++ b/srv/PlanToolPath.srv
@@ -0,0 +1,13 @@
+# Tool path planning pipeline YAML configuration
+# This parameter can either be a YAML-formatted string or an absolute path to a YAML file
+string config
+
+# File path of the mesh resource
+string mesh_file
+
+# TF frame to which the mesh is relative
+string mesh_frame
+---
+bool success
+string message
+noether_ros/ToolPaths[] tool_paths
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
new file mode 100644
index 0000000..ebb3dd3
--- /dev/null
+++ b/test/CMakeLists.txt
@@ -0,0 +1,18 @@
+# Integration tests
+find_package(ament_cmake_ros REQUIRED)
+find_package(launch_testing_ament_cmake REQUIRED)
+
+# Create a function for adding isolated launch tests
+function(add_ros_isolated_launch_test path)
+  set(RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py")
+  add_launch_test(
+    "${path}"
+    RUNNER
+    "${RUNNER}"
+    ${ARGN}
+  )
+endfunction()
+
+# Add a test for the TPP server
+add_ros_isolated_launch_test(test_server.py)
+install(FILES dome.ply DESTINATION share/${PROJECT_NAME}/test)
diff --git a/test/dome.ply b/test/dome.ply
new file mode 100644
index 0000000..9254b34
Binary files /dev/null and b/test/dome.ply differ
diff --git a/test/test_server.py b/test/test_server.py
new file mode 100644
index 0000000..90f3883
--- /dev/null
+++ b/test/test_server.py
@@ -0,0 +1,111 @@
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node as LaunchNode
+import launch_testing
+from noether_ros.srv import PlanToolPath
+import os
+import rclpy
+from rclpy.node import Node
+import unittest
+
+
+def generate_test_description():
+    return LaunchDescription([
+            LaunchNode(
+                package='noether_ros',
+                executable='noether_ros_tool_path_planning_server',
+                output='screen'
+        ),
+        launch_testing.actions.ReadyToTest()
+    ])
+
+
+config_str = R"""
+mesh_modifiers:
+  - name: NormalsFromMeshFaces
+tool_path_planner:
+  name: Multi
+  gui_plugin_name: CrossHatchPlaneSlicer
+  planners:
+    - name: PlaneSlicer
+      direction_generator:
+        name: PrincipalAxis
+        rotation_offset: 1.571
+      origin_generator:
+        name: AABBCenter
+      line_spacing: 0.1
+      point_spacing: 0.025
+      min_hole_size: 0.100
+      search_radius: 0.100
+      min_segment_size: 0.100
+      bidirectional: true
+    - name: PlaneSlicer
+      direction_generator:
+        name: PCARotated
+        direction_generator:
+          name: PrincipalAxis
+          rotation_offset: 1.571
+        rotation_offset: 1.571
+      origin_generator:
+        name: AABBCenter
+      line_spacing: 0.100
+      point_spacing: 0.025
+      min_hole_size: 0.100
+      search_radius: 0.100
+      min_segment_size: 0.100
+      bidirectional: true
+tool_path_modifiers:
+  - name: SnakeOrganization
+"""
+
+# Active test
+class TestToolPathPlanningClient(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(self):
+        rclpy.init()
+        self.node = rclpy.create_node("test_client")
+
+    @classmethod
+    def tearDownClass(self):
+        self.node.destroy_node()
+        rclpy.shutdown()
+
+    def test_tool_path_planning_client(self):
+        # Create the client
+        client = self.node.create_client(PlanToolPath, 'plan_tool_path')
+
+        # Check that the service is available
+        self.assertTrue(client.wait_for_service(timeout_sec=5.0), "Service does not exist")
+
+        # Create the request
+        request = PlanToolPath.Request()
+
+        # Add the TPP config string to the request
+        request.config = config_str
+
+        # Add the mesh file to the request
+        pkg_prefix = get_package_share_directory('noether_ros')
+        request.mesh_file = os.path.join(pkg_prefix, 'test', 'dome.ply')
+
+        # Check that the mesh file is valid
+        self.assertIsNotNone(pkg_prefix)
+        self.assertTrue(os.path.exists(request.mesh_file))
+
+        # Call the service
+        future = client.call_async(request)
+        rclpy.spin_until_future_complete(self.node, future)
+
+        # Check the response
+        response = future.result()
+        self.assertIsNotNone(response, "No response received")
+        self.assertTrue(response.success, response.message)
+        self.assertEqual(len(response.tool_paths), 1)
+        for tool_paths in response.tool_paths:
+            self.assertGreaterEqual(len(tool_paths.tool_paths), 1)
+            for tool_path in tool_paths.tool_paths:
+                self.assertGreaterEqual(len(tool_path.segments), 1)
+                for segment in tool_path.segments:
+                    self.assertGreaterEqual(len(segment.poses), 1)
+
+        self.node.destroy_client(client)