diff --git a/.devcontainer/nouveau/devcontainer.json b/.devcontainer/nouveau/devcontainer.json
index 1fbdde5..9b27e35 100644
--- a/.devcontainer/nouveau/devcontainer.json
+++ b/.devcontainer/nouveau/devcontainer.json
@@ -22,76 +22,6 @@
   },
   "customizations": {
     "vscode": {
-      "settings": {
-        "files.associations": {
-          "*.repos": "yaml",
-          "*.world": "xml",
-          "*.xacro": "xml",
-          "*.srdf": "xml",
-          "*.rviz": "yaml",
-          "*.config": "xml",
-          "*.sdf": "xml"
-        },
-        "terminal.integrated.defaultProfile.linux": "bash",
-        "files.insertFinalNewline": true,
-        "files.trimTrailingWhitespace": true,
-        "editor.formatOnSave": true,
-        "editor.tabSize": 2,
-        "autoDocstring.startOnNewLine": false,
-        "autoDocstring.docstringFormat": "google-notypes",
-        "python.autoComplete.extraPaths": [
-          "/opt/ros/iron/lib/python3.10/site-packages/",
-          "/opt/ros/iron/local/lib/python3.10/dist-packages/",
-          "${workspaceFolder}/install/"
-        ],
-        "python.analysis.extraPaths": [
-          "/opt/ros/iron/lib/python3.10/site-packages/",
-          "/opt/ros/iron/local/lib/python3.10/dist-packages/",
-          "${workspaceFolder}/install/"
-        ],
-        "C_Cpp.default.intelliSenseMode": "linux-gcc-x86",
-        "C_Cpp.clang_format_fallbackStyle": "Google",
-        "C_Cpp.codeAnalysis.clangTidy.enabled": true,
-        "C_Cpp.codeAnalysis.clangTidy.runAutomatically": true,
-        "clang-format.executable": "/usr/bin/clang-format-14",
-        "[cpp]": {
-          "editor.rulers": [100],
-          "editor.tabSize": 2,
-          "editor.defaultFormatter": "xaver.clang-format"
-        },
-        "[python]": {
-          "editor.tabSize": 4,
-          "editor.rulers": [90],
-          "editor.formatOnSave": true,
-          "editor.codeActionsOnSave": {
-            "source.fixAll": true,
-            "source.organizeImports": true
-          },
-          "editor.defaultFormatter": "ms-python.black-formatter"
-        },
-        "[dockerfile]": {
-          "editor.quickSuggestions": {
-            "strings": true
-          },
-          "editor.defaultFormatter": "ms-azuretools.vscode-docker",
-          "editor.tabSize": 4
-        },
-        "[json]": {
-          "editor.defaultFormatter": "esbenp.prettier-vscode"
-        },
-        "[xml]": {
-          "editor.defaultFormatter": "redhat.vscode-xml"
-        },
-        "[markdown]": {
-          "editor.rulers": [80],
-          "editor.defaultFormatter": "DavidAnson.vscode-markdownlint"
-        },
-        "search.exclude": {
-          "**/build": true,
-          "**/install": true,
-          "**/log": true
-        }
-      },
       "extensions": [
         "ms-azuretools.vscode-docker",
         "ms-python.python",
diff --git a/.devcontainer/nvidia/devcontainer.json b/.devcontainer/nvidia/devcontainer.json
index c9e580f..bdc9f5f 100644
--- a/.devcontainer/nvidia/devcontainer.json
+++ b/.devcontainer/nvidia/devcontainer.json
@@ -26,76 +26,6 @@
   },
   "customizations": {
     "vscode": {
-      "settings": {
-        "files.associations": {
-          "*.repos": "yaml",
-          "*.world": "xml",
-          "*.xacro": "xml",
-          "*.srdf": "xml",
-          "*.rviz": "yaml",
-          "*.config": "xml",
-          "*.sdf": "xml"
-        },
-        "terminal.integrated.defaultProfile.linux": "bash",
-        "files.insertFinalNewline": true,
-        "files.trimTrailingWhitespace": true,
-        "editor.formatOnSave": true,
-        "editor.tabSize": 2,
-        "autoDocstring.startOnNewLine": false,
-        "autoDocstring.docstringFormat": "google-notypes",
-        "python.autoComplete.extraPaths": [
-          "/opt/ros/iron/lib/python3.10/site-packages/",
-          "/opt/ros/iron/local/lib/python3.10/dist-packages/",
-          "${workspaceFolder}/install/"
-        ],
-        "python.analysis.extraPaths": [
-          "/opt/ros/iron/lib/python3.10/site-packages/",
-          "/opt/ros/iron/local/lib/python3.10/dist-packages/",
-          "${workspaceFolder}/install/"
-        ],
-        "C_Cpp.default.intelliSenseMode": "linux-gcc-x86",
-        "C_Cpp.clang_format_fallbackStyle": "Google",
-        "C_Cpp.codeAnalysis.clangTidy.enabled": true,
-        "C_Cpp.codeAnalysis.clangTidy.runAutomatically": true,
-        "clang-format.executable": "/usr/bin/clang-format-14",
-        "[cpp]": {
-          "editor.rulers": [100],
-          "editor.tabSize": 2,
-          "editor.defaultFormatter": "xaver.clang-format"
-        },
-        "[python]": {
-          "editor.tabSize": 4,
-          "editor.rulers": [90],
-          "editor.formatOnSave": true,
-          "editor.codeActionsOnSave": {
-            "source.fixAll": true,
-            "source.organizeImports": true
-          },
-          "editor.defaultFormatter": "ms-python.black-formatter"
-        },
-        "[dockerfile]": {
-          "editor.quickSuggestions": {
-            "strings": true
-          },
-          "editor.defaultFormatter": "ms-azuretools.vscode-docker",
-          "editor.tabSize": 4
-        },
-        "[json]": {
-          "editor.defaultFormatter": "esbenp.prettier-vscode"
-        },
-        "[xml]": {
-          "editor.defaultFormatter": "redhat.vscode-xml"
-        },
-        "[markdown]": {
-          "editor.rulers": [80],
-          "editor.defaultFormatter": "DavidAnson.vscode-markdownlint"
-        },
-        "search.exclude": {
-          "**/build": true,
-          "**/install": true,
-          "**/log": true
-        }
-      },
       "extensions": [
         "ms-azuretools.vscode-docker",
         "ms-python.python",
diff --git a/.gitignore b/.gitignore
index 259148f..7e1fc9a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,32 +1,4 @@
-# Prerequisites
-*.d
-
-# Compiled Object files
-*.slo
-*.lo
-*.o
-*.obj
-
-# Precompiled Headers
-*.gch
-*.pch
-
-# Compiled Dynamic libraries
-*.so
-*.dylib
-*.dll
-
-# Fortran module files
-*.mod
-*.smod
-
-# Compiled Static libraries
-*.lai
-*.la
-*.a
-*.lib
-
-# Executables
-*.exe
-*.out
-*.app
+# ROS 2
+build
+install
+log
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000..25ed846
--- /dev/null
+++ b/.vscode/c_cpp_properties.json
@@ -0,0 +1,18 @@
+{
+  "configurations": [
+    {
+      "name": "Linux",
+      "includePath": [
+        "${workspaceFolder}/**",
+        "/opt/ros/iron/include/**",
+        "/usr/include/gz/**"
+      ],
+      "defines": [],
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "c99",
+      "cppStandard": "c++20",
+      "intelliSenseMode": "linux-gcc-x86"
+    }
+  ],
+  "version": 4
+}
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..7a114d0
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,70 @@
+{
+  "files.associations": {
+    "*.repos": "yaml",
+    "*.world": "xml",
+    "*.xacro": "xml",
+    "*.srdf": "xml",
+    "*.rviz": "yaml",
+    "*.config": "xml",
+    "*.sdf": "xml"
+  },
+  "terminal.integrated.defaultProfile.linux": "bash",
+  "files.insertFinalNewline": true,
+  "files.trimTrailingWhitespace": true,
+  "editor.formatOnSave": true,
+  "editor.tabSize": 2,
+  "autoDocstring.startOnNewLine": false,
+  "autoDocstring.docstringFormat": "google-notypes",
+  "python.autoComplete.extraPaths": [
+    "/opt/ros/iron/lib/python3.10/site-packages/",
+    "/opt/ros/iron/local/lib/python3.10/dist-packages/",
+    "${workspaceFolder}/install/"
+  ],
+  "python.analysis.extraPaths": [
+    "/opt/ros/iron/lib/python3.10/site-packages/",
+    "/opt/ros/iron/local/lib/python3.10/dist-packages/",
+    "${workspaceFolder}/install/"
+  ],
+  "C_Cpp.default.intelliSenseMode": "linux-gcc-x86",
+  "C_Cpp.clang_format_fallbackStyle": "Google",
+  "C_Cpp.codeAnalysis.clangTidy.enabled": true,
+  "C_Cpp.codeAnalysis.clangTidy.codeAction.formatFixes": true,
+  "clang-format.executable": "/usr/bin/clang-format-14",
+  "[cpp]": {
+    "editor.rulers": [100],
+    "editor.tabSize": 2,
+    "editor.defaultFormatter": "xaver.clang-format"
+  },
+  "[python]": {
+    "editor.tabSize": 4,
+    "editor.rulers": [90],
+    "editor.formatOnSave": true,
+    "editor.codeActionsOnSave": {
+      "source.fixAll": "explicit",
+      "source.organizeImports": "explicit"
+    },
+    "editor.defaultFormatter": "ms-python.black-formatter"
+  },
+  "[dockerfile]": {
+    "editor.quickSuggestions": {
+      "strings": true
+    },
+    "editor.defaultFormatter": "ms-azuretools.vscode-docker",
+    "editor.tabSize": 4
+  },
+  "[json]": {
+    "editor.defaultFormatter": "esbenp.prettier-vscode"
+  },
+  "[xml]": {
+    "editor.defaultFormatter": "redhat.vscode-xml"
+  },
+  "[markdown]": {
+    "editor.rulers": [80],
+    "editor.defaultFormatter": "DavidAnson.vscode-markdownlint"
+  },
+  "search.exclude": {
+    "**/build": true,
+    "**/install": true,
+    "**/log": true
+  }
+}
diff --git a/.vscode/tasks.json b/.vscode/tasks.json
new file mode 100644
index 0000000..7ead665
--- /dev/null
+++ b/.vscode/tasks.json
@@ -0,0 +1,47 @@
+{
+  "version": "2.0.0",
+  "tasks": [
+    {
+      "label": "ROS 2: Build",
+      "detail": "Build the workspace using colcon",
+      "type": "shell",
+      "command": "colcon build --symlink-install && source install/setup.bash",
+      "group": {
+        "kind": "build",
+        "isDefault": true
+      },
+      "problemMatcher": ["$gcc"]
+    },
+    {
+      "label": "ROS 2: Test",
+      "detail": "Run system tests using colcon",
+      "type": "shell",
+      "command": "colcon test && colcon test-result --all",
+      "group": {
+        "kind": "test",
+        "isDefault": true
+      }
+    },
+    {
+      "label": "ROS 2: Create ament_cmake package",
+      "detail": "Create a new ROS 2 ament_cmake package",
+      "type": "shell",
+      "command": "ros2 pkg create --destination-directory src --build-type ament_cmake ${input:packageName}",
+      "problemMatcher": []
+    },
+    {
+      "label": "ROS 2: Create ament_python package",
+      "detail": "Create a new ROS 2 ament_python package",
+      "type": "shell",
+      "command": "ros2 pkg create --destination-directory src --build-type ament_python ${input:packageName}",
+      "problemMatcher": []
+    }
+  ],
+  "inputs": [
+    {
+      "id": "packageName",
+      "type": "promptString",
+      "description": "Package name"
+    }
+  ]
+}