diff --git a/.circleci/config.yml b/.circleci/config.yml
index 1fa5c7b80ef..5041d365ac8 100644
--- a/.circleci/config.yml
+++ b/.circleci/config.yml
@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v32\
+              - "<< parameters.key >>-v33\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v32\
+              - "<< parameters.key >>-v33\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v32\
+            key: "<< parameters.key >>-v33\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\
diff --git a/.devcontainer/caddy/srv/assets/foxglove/nav2_layout.json b/.devcontainer/caddy/srv/assets/foxglove/nav2_layout.json
index f92821fac49..be9d963a25d 100644
--- a/.devcontainer/caddy/srv/assets/foxglove/nav2_layout.json
+++ b/.devcontainer/caddy/srv/assets/foxglove/nav2_layout.json
@@ -460,4 +460,4 @@
       "direction": "row",
       "splitPercentage": 74.87855655794587
     }
-  }
\ No newline at end of file
+  }
diff --git a/.devcontainer/caddy/srv/nav2/github-markdown.css b/.devcontainer/caddy/srv/nav2/github-markdown.css
index 049cae6b291..7cb35e4a33e 100644
--- a/.devcontainer/caddy/srv/nav2/github-markdown.css
+++ b/.devcontainer/caddy/srv/nav2/github-markdown.css
@@ -1099,4 +1099,4 @@
 
   .markdown-body ::-webkit-calendar-picker-indicator {
     filter: invert(50%);
-  }
\ No newline at end of file
+  }
diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md
index 2a2e2339ee5..5b818d8e6e5 100644
--- a/.github/PULL_REQUEST_TEMPLATE.md
+++ b/.github/PULL_REQUEST_TEMPLATE.md
@@ -13,6 +13,13 @@
 
 ---
 
+## Description of testing performed
+<!--
+  For example: Linting validation using -> pre-commit run --all,
+    Package testing using -> colcon test --packages-select <modified package>,
+    or functional testing of changes on the robot or in simulation
+-->
+
 ## Description of contribution in a few bullet points
 
 <!--
diff --git a/.github/mergify.yml b/.github/mergify.yml
index 332495c67f9..9abbb04672f 100644
--- a/.github/mergify.yml
+++ b/.github/mergify.yml
@@ -7,7 +7,7 @@ pull_request_rules:
       backport:
         branches:
           - jazzy
-  
+
   - name: backport to iron at reviewers discretion
     conditions:
       - base=main
@@ -16,7 +16,7 @@ pull_request_rules:
       backport:
         branches:
           - iron
-  
+
   - name: backport to humble at reviewers discretion
     conditions:
       - base=main
diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml
index c181fe25f4c..62645e662aa 100644
--- a/.github/workflows/lint.yml
+++ b/.github/workflows/lint.yml
@@ -11,11 +11,34 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-          linter: [xmllint, cpplint, uncrustify, pep257, flake8]
+          linter: [xmllint, cpplint, uncrustify, pep257, flake8, mypy]
     steps:
     - uses: actions/checkout@v4
+
+    - name: Install typeshed for mypy
+      if: matrix.linter == 'mypy'
+      run: sudo apt update && sudo apt install -y python3-typeshed
+
     - uses: ros-tooling/action-ros-lint@v0.1
       with:
         linter: ${{ matrix.linter }}
         distribution: rolling
         package-name: "*"
+        arguments: ${{ matrix.linter == 'mypy' && '--config tools/pyproject.toml' || '' }}
+
+  pre-commit:
+    name: pre-commit
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+      - uses: actions/setup-python@v5
+      - uses: pre-commit/action@v3.0.1
+        env:
+          SKIP: >-
+            ament_lint_cmake,
+            ament_cpplint,
+            ament_uncrustify,
+            ament_xmllint,
+            ament_flake8,
+            ament_pep257,
+            ament_mypy
diff --git a/.gitignore b/.gitignore
index 1247ce1b159..203501ea402 100644
--- a/.gitignore
+++ b/.gitignore
@@ -69,4 +69,3 @@ Session.vim
 
 # Vim Temporary
 .netrwhist
-
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
new file mode 100644
index 00000000000..dfb9874a89a
--- /dev/null
+++ b/.pre-commit-config.yaml
@@ -0,0 +1,104 @@
+
+# To use:
+#
+#     pre-commit run -a
+#
+# Or:
+#
+#     pre-commit install  # (runs every time you commit in git)
+#
+# To update this file:
+#
+#     pre-commit autoupdate
+#
+# See https://github.com/pre-commit/pre-commit
+exclude: ".pgm$|.svg$"
+repos:
+-   repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v5.0.0
+    hooks:
+      - id: check-added-large-files
+      - id: check-ast
+      - id: check-case-conflict
+      - id: check-merge-conflict
+      - id: check-symlinks
+      - id: check-xml
+      - id: check-yaml
+        args: ["--allow-multiple-documents"]
+      - id: debug-statements
+      - id: end-of-file-fixer
+      - id: forbid-submodules
+      - id: mixed-line-ending
+      - id: trailing-whitespace
+        exclude_types: [rst]
+      - id: fix-byte-order-marker
+-   repo: https://github.com/pycqa/isort
+    rev: 6.0.1
+    hooks:
+      - id: isort
+        args: ["tools/pyproject.toml"]
+        name: isort (python)
+
+-   repo: https://github.com/codespell-project/codespell
+    rev: v2.4.1
+    hooks:
+    -   id: codespell
+        additional_dependencies:
+        -   tomli
+        args:
+            [--toml=./tools/pyproject.toml]
+-   repo: https://github.com/python-jsonschema/check-jsonschema
+    rev: 0.31.1
+    hooks:
+      - id: check-github-workflows
+        args: ["--verbose"]
+      - id: check-github-actions
+        args: ["--verbose"]
+      - id: check-dependabot
+        args: ["--verbose"]
+-   repo: local
+    hooks:
+    -   id: ament_lint_cmake
+        name: ament_lint_cmake
+        description: Check CMake code style using cmakelint.
+        language: system
+        types: [cmake]
+        entry: ament_lint_cmake
+    -   id: ament_cpplint
+        name: ament_cpplint
+        description: Code style checking using cpplint.
+        language: system
+        types: [c++]
+        entry: ament_cpplint
+    -   id: ament_uncrustify
+        name: ament_uncrustify
+        description: Code style checking using uncrustify.
+        language: system
+        types: [c++]
+        args: ["--reformat"]
+        entry: ament_uncrustify
+    -   id: ament_xmllint
+        name: ament_xmllint
+        description: Check XML markup using xmllint.
+        language: system
+        types: [xml]
+        entry: ament_xmllint
+    -   id: ament_flake8
+        name: ament_flake8
+        description: Check Python code style using flake8.
+        language: system
+        types: [python]
+        entry: ament_flake8
+    -   id: ament_pep257
+        name: ament_pep257
+        description: Check Python code style using pep257.
+        language: system
+        types: [python]
+        entry: ament_pep257
+    -   id: ament_mypy
+        name: ament_mypy
+        description: Check Python code style using mypy.
+        language: system
+        types: [python]
+        args: ["--config", "tools/pyproject.toml"]
+        entry: ament_mypy
diff --git a/Dockerfile b/Dockerfile
index 1f959ff1f7f..fb3555eb8fb 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -168,7 +168,7 @@ RUN mkdir -p $ROOT_SRV
 
 # install demo dependencies
 RUN apt-get update && apt-get install -y \
-      ros-$ROS_DISTRO-rviz2 
+      ros-$ROS_DISTRO-rviz2
 
 # install gzweb dependacies
 RUN apt-get install -y --no-install-recommends \
diff --git a/README.md b/README.md
index dfadfc6a0cc..424348f2db1 100644
--- a/README.md
+++ b/README.md
@@ -26,7 +26,7 @@ Please visit our [documentation site](https://docs.nav2.org/). [Please visit our
 
 ## Our Sponsors
 
-Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community. 
+Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community.
 
 <p align="center">
   <img src="doc/sponsors_jan_2025.png" />
@@ -46,7 +46,7 @@ If you use the navigation framework, an algorithm from this repository, or ideas
 please cite this work in your papers!
 
  - S. Macenski, F. Martín, R. White, J. Clavero. [**The Marathon 2: A Navigation System**](https://arxiv.org/abs/2003.00368). IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020.
- 
+
  ```bibtex
  @InProceedings{macenski2020marathon2,
    title = {The Marathon 2: A Navigation System},
@@ -64,7 +64,7 @@ If you use **any** of the algorithms in Nav2 or the analysis of the algorithms i
 
 ```bibtex
   @article{macenski2023survey,
-        title={From the desks of ROS maintainers: A survey of modern & capable mobile robotics algorithms in the robot operating system 2}, 
+        title={From the desks of ROS maintainers: A survey of modern & capable mobile robotics algorithms in the robot operating system 2},
         author={S. Macenski, T. Moore, DV Lu, A. Merzlyakov, M. Ferguson},
         year={2023},
         journal = {Robotics and Autonomous Systems}
@@ -77,7 +77,7 @@ If you use the Smac Planner (Hybrid A*, State Lattice, 2D), please cite this wor
 
 ```bibtex
 @article{macenski2024smac,
-      title={Open-Source, Cost-Aware Kinematically Feasible Planning for Mobile and Surface Robotics}, 
+      title={Open-Source, Cost-Aware Kinematically Feasible Planning for Mobile and Surface Robotics},
       author={Steve Macenski and Matthew Booker and Josh Wallace},
       year={2024},
       journal = {Arxiv}
@@ -90,7 +90,7 @@ If you use the Regulated Pure Pursuit Controller algorithm or software from this
 
 ```bibtex
 @article{macenski2023regulated,
-      title={Regulated Pure Pursuit for Robot Path Tracking}, 
+      title={Regulated Pure Pursuit for Robot Path Tracking},
       author={Steve Macenski and Shrijit Singh and Francisco Martin and Jonatan Gines},
       year={2023},
       journal = {Autonomous Robots}
@@ -113,38 +113,38 @@ If you use the Regulated Pure Pursuit Controller algorithm or software from this
 
 ## Build Status
 
-| Package | humble Source | humble Debian | iron Source | iron Debian | jazzy Source | jazzy Debian | 
+| Package | humble Source | humble Debian | iron Source | iron Debian | jazzy Source | jazzy Debian |
 | :---: | :---: | :---: | :---: | :---: | :---: | :---: |
-| navigation2 | [![Build Status](https://build.ros2.org/job/Hsrc_uj__navigation2__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__navigation2__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__navigation2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__navigation2__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__navigation2__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__navigation2__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__navigation2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__navigation2__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__navigation2__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__navigation2__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__navigation2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__navigation2__ubuntu_noble_amd64__binary/) | 
-| nav2_amcl | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_amcl__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_amcl__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_amcl__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_amcl__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_amcl__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_amcl__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/) | 
-| nav2_behavior_tree | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_behavior_tree__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_behavior_tree__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_behavior_tree__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_behavior_tree__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_behavior_tree__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_behavior_tree__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/) | 
-| nav2_behaviors | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_behaviors__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_behaviors__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_behaviors__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_behaviors__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_behaviors__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_behaviors__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/) | 
-| nav2_bringup | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_bringup__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_bringup__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_bringup__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_bringup__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_bringup__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_bringup__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/) | 
-| nav2_bt_navigator | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_bt_navigator__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_bt_navigator__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_bt_navigator__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_bt_navigator__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_bt_navigator__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_bt_navigator__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/) | 
-| nav2_collision_monitor | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_collision_monitor__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_collision_monitor__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_collision_monitor__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_collision_monitor__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_collision_monitor__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_collision_monitor__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/) | 
-| nav2_common | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_common__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_common__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_common__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_common__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_common__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_common__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_common__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_common__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_common__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_common__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_common__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_common__ubuntu_noble_amd64__binary/) | 
-| nav2_constrained_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_constrained_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_constrained_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/) | 
-| nav2_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_controller__ubuntu_noble_amd64__binary/) | 
-| nav2_core | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_core__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_core__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_core__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_core__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_core__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_core__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_core__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_core__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_core__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_core__ubuntu_noble_amd64__binary/) | 
-| nav2_costmap_2d | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_costmap_2d__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_costmap_2d__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_costmap_2d__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_costmap_2d__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_costmap_2d__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_costmap_2d__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/) | 
-| nav2_docking | [![Build Status](https://build.ros2.org/job/Hsrc_uj__opennav_docking__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__opennav_docking__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__opennav_docking__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__opennav_docking__ubuntu_jammy_amd64__binary/) | N/A | N/A | [![Build Status](https://build.ros2.org/job/Jsrc_un__opennav_docking__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__opennav_docking__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__opennav_docking__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__opennav_docking__ubuntu_noble_amd64__binary/) | 
-| nav2_dwb_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_dwb_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_dwb_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_dwb_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_dwb_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_dwb_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_dwb_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/) | 
-| nav2_graceful_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_graceful_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_graceful_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_graceful_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_graceful_controller__ubuntu_jammy_amd64__binary/) | N/A | N/A | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_graceful_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_graceful_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/) | 
-| nav2_lifecycle_manager | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_lifecycle_manager__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_lifecycle_manager__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/) | 
-| nav2_map_server | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_map_server__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_map_server__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_map_server__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_map_server__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_map_server__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_map_server__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/) | 
-| nav2_mppi_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_mppi_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_mppi_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_mppi_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_mppi_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_mppi_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_mppi_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/) | 
-| nav2_msgs | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_msgs__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_msgs__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_msgs__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_msgs__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_msgs__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_msgs__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/) | 
-| nav2_navfn_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_navfn_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_navfn_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_navfn_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_navfn_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_navfn_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_navfn_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/) | 
-| nav2_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_planner__ubuntu_noble_amd64__binary/) | 
-| nav2_regulated_pure_pursuit | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/) | 
-| nav2_rotation_shim_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/) | 
-| nav2_rviz_plugins | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_rviz_plugins__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_rviz_plugins__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/) | 
-| nav2_simple_commander | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_simple_commander__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_simple_commander__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_simple_commander__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_simple_commander__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_simple_commander__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_simple_commander__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/) | 
-| nav2_smac_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_smac_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_smac_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_smac_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_smac_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_smac_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_smac_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/) | 
-| nav2_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/) | 
-| nav2_system_tests | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_system_tests__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_system_tests__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_system_tests__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_system_tests__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_system_tests__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_system_tests__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/) | 
-| nav2_theta_star_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_theta_star_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_theta_star_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/) | 
-| nav2_util | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_util__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_util__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_util__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_util__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_util__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_util__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_util__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_util__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_util__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_util__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_util__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_util__ubuntu_noble_amd64__binary/) | 
-| nav2_velocity_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_velocity_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_velocity_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/) | 
-| nav2_voxel_grid | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_voxel_grid__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_voxel_grid__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_voxel_grid__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_voxel_grid__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_voxel_grid__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_voxel_grid__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/) | 
-| nav2_waypoint_follower | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_waypoint_follower__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_waypoint_follower__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/) | 
+| navigation2 | [![Build Status](https://build.ros2.org/job/Hsrc_uj__navigation2__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__navigation2__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__navigation2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__navigation2__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__navigation2__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__navigation2__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__navigation2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__navigation2__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__navigation2__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__navigation2__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__navigation2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__navigation2__ubuntu_noble_amd64__binary/) |
+| nav2_amcl | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_amcl__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_amcl__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_amcl__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_amcl__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_amcl__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_amcl__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/) |
+| nav2_behavior_tree | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_behavior_tree__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_behavior_tree__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_behavior_tree__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_behavior_tree__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_behavior_tree__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_behavior_tree__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/) |
+| nav2_behaviors | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_behaviors__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_behaviors__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_behaviors__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_behaviors__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_behaviors__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_behaviors__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/) |
+| nav2_bringup | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_bringup__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_bringup__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_bringup__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_bringup__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_bringup__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_bringup__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/) |
+| nav2_bt_navigator | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_bt_navigator__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_bt_navigator__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_bt_navigator__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_bt_navigator__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_bt_navigator__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_bt_navigator__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/) |
+| nav2_collision_monitor | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_collision_monitor__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_collision_monitor__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_collision_monitor__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_collision_monitor__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_collision_monitor__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_collision_monitor__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/) |
+| nav2_common | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_common__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_common__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_common__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_common__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_common__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_common__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_common__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_common__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_common__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_common__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_common__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_common__ubuntu_noble_amd64__binary/) |
+| nav2_constrained_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_constrained_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_constrained_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/) |
+| nav2_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_controller__ubuntu_noble_amd64__binary/) |
+| nav2_core | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_core__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_core__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_core__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_core__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_core__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_core__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_core__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_core__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_core__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_core__ubuntu_noble_amd64__binary/) |
+| nav2_costmap_2d | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_costmap_2d__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_costmap_2d__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_costmap_2d__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_costmap_2d__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_costmap_2d__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_costmap_2d__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/) |
+| nav2_docking | [![Build Status](https://build.ros2.org/job/Hsrc_uj__opennav_docking__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__opennav_docking__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__opennav_docking__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__opennav_docking__ubuntu_jammy_amd64__binary/) | N/A | N/A | [![Build Status](https://build.ros2.org/job/Jsrc_un__opennav_docking__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__opennav_docking__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__opennav_docking__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__opennav_docking__ubuntu_noble_amd64__binary/) |
+| nav2_dwb_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_dwb_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_dwb_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_dwb_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_dwb_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_dwb_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_dwb_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/) |
+| nav2_graceful_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_graceful_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_graceful_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_graceful_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_graceful_controller__ubuntu_jammy_amd64__binary/) | N/A | N/A | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_graceful_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_graceful_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/) |
+| nav2_lifecycle_manager | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_lifecycle_manager__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_lifecycle_manager__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/) |
+| nav2_map_server | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_map_server__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_map_server__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_map_server__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_map_server__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_map_server__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_map_server__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/) |
+| nav2_mppi_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_mppi_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_mppi_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_mppi_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_mppi_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_mppi_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_mppi_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/) |
+| nav2_msgs | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_msgs__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_msgs__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_msgs__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_msgs__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_msgs__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_msgs__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/) |
+| nav2_navfn_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_navfn_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_navfn_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_navfn_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_navfn_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_navfn_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_navfn_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/) |
+| nav2_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_planner__ubuntu_noble_amd64__binary/) |
+| nav2_regulated_pure_pursuit | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/) |
+| nav2_rotation_shim_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/) |
+| nav2_rviz_plugins | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_rviz_plugins__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_rviz_plugins__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/) |
+| nav2_simple_commander | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_simple_commander__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_simple_commander__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_simple_commander__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_simple_commander__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_simple_commander__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_simple_commander__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/) |
+| nav2_smac_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_smac_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_smac_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_smac_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_smac_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_smac_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_smac_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/) |
+| nav2_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/) |
+| nav2_system_tests | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_system_tests__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_system_tests__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_system_tests__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_system_tests__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_system_tests__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_system_tests__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/) |
+| nav2_theta_star_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_theta_star_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_theta_star_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/) |
+| nav2_util | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_util__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_util__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_util__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_util__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_util__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_util__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_util__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_util__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_util__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_util__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_util__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_util__ubuntu_noble_amd64__binary/) |
+| nav2_velocity_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_velocity_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_velocity_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/) |
+| nav2_voxel_grid | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_voxel_grid__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_voxel_grid__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_voxel_grid__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_voxel_grid__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_voxel_grid__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_voxel_grid__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/) |
+| nav2_waypoint_follower | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_waypoint_follower__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_waypoint_follower__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/) |
diff --git a/doc/development/codespaces.md b/doc/development/codespaces.md
index 841db7532db..7d7616ace7b 100644
--- a/doc/development/codespaces.md
+++ b/doc/development/codespaces.md
@@ -20,4 +20,4 @@ TODO: gazebo example with gzweb
 
 # References
 
-TODO: links to more info
\ No newline at end of file
+TODO: links to more info
diff --git a/doc/requirements/_template_requirement.md b/doc/requirements/_template_requirement.md
index 6b751e13921..aba4eeaf8e9 100644
--- a/doc/requirements/_template_requirement.md
+++ b/doc/requirements/_template_requirement.md
@@ -3,7 +3,7 @@ The \<navigation system> should be able to \<shall> \<do something>
 
 ## More details
 - Why is this needed?
-- What is the expected user interaction? 
+- What is the expected user interaction?
 - What use case does this map to?
 - Are there any non-functional requirements (build system, tools, performance, etc)
 
diff --git a/doc/requirements/requirements.md b/doc/requirements/requirements.md
index 1ca7171c6a0..41e13483973 100644
--- a/doc/requirements/requirements.md
+++ b/doc/requirements/requirements.md
@@ -156,7 +156,7 @@ Id | Handle | Priority | Description | Notes
 TP001 | Target Platforms.Operating Systems.Ubuntu | 1 | The Navigation System MUST support Ubuntu Desktop 16.04 and Ubuntu Desktop 18.04
 TP002 | Target Platforms.Operating Systems.MacOS | 1 | The Navigation System MUST support MacOS 10.13 (High Sierra) and MacOS 10.14 (Mohave)
 TP003 | Target Platforms.Operating Systems.Windows | 1 | The Navigation System MUST support Windows 10 Professional
-TP004 | Target Platforms.Operating Systems.Clear Linux | 1 | The Navigation System SHOULD support the Intel's Clear Linux distribution | Clear Linux uses a continuous deployment model. 
+TP004 | Target Platforms.Operating Systems.Clear Linux | 1 | The Navigation System SHOULD support the Intel's Clear Linux distribution | Clear Linux uses a continuous deployment model.
 TP005 | Target Platforms.CPU.Word Size | 1 | The Navigation System SHALL support 64-bit processors | Don't assume a specific pointer size
 TP006 | Target Platforms.Minimum Platform | 1 | *TODO: Should we specify a minimum target platform? Or, should this be expressed as minimum platform requirements?*
 
@@ -178,7 +178,7 @@ MP005 | Mission Planning.Navigation Commands.Enqueue | 2 | The Mission Plan SHOU
 MP006 | Mission Planning.Navigation Commands.Follow | 2 | The Mission Plan SHOULD be able to convey the information required for a robot to be able to follow another specified robot. | This one doesn't have a completion state (reaching the goal), unless it specifies additional information such as "follow until destination reached."
 MP007 | Mission Planning.Navigation Commands.Maintain Pose | 1 | The Mission Plan SHOULD be able to convey the information required for a robot to maintain its current pose. | Could be indefinite or time-based.
 MP008 | Mission Planning.Navigation Commands.Park | 2 | The Mission Plan SHOULD be able to convey the information required for a robot to park itself. | The implementation of the parking command could interact with the robot to cause it, for example, to shut down or enter a low-power state.
-MP009 | Mission Planning.Navigation Commands.Dock to Charger | 2 | The Mission Plan SHOULD be able to convey the information required for a robot to dock to a specific charging station. 
+MP009 | Mission Planning.Navigation Commands.Dock to Charger | 2 | The Mission Plan SHOULD be able to convey the information required for a robot to dock to a specific charging station.
 MP010 | Mission Planning.Policy | 1 | The Mission Plan SHOULD be able to express information about how and when the navigation commands are to be carried out. | Time and safety constraints.
 MP011 | Mission Planning.Policy.Time.Initiation | 1 | The Mission Plan SHOULD be able to convey when a mission should begin.
 MP012 | Mission Planning.Policy.Time.Completion | 1 | The Mission Plan SHOULD be able to convey by when a mission should end.
@@ -216,7 +216,7 @@ PLN002 | Planning.Inputs.Navigation Command | 1 | The Planning Module SHALL rece
 PLN003 | Planning.Inputs.Policy | 1 | The Planning Module SHALL receive policy information associated with the Navigation Command to execute. | This could be global policy and/or per-command policy. Policy could contain, for example, a list of conventions for the robot to follow (navigate on the right side of a path, for example).
 PLN004 | Planning.Inputs.Mapping.Maps | 1 | The Planning Module MUST have access to one or more maps available that describe the robot's environment.
 PLN005 | Planning.Inputs.Perception.Sensory Input | 1 | The Planning Module MUST have access to data from the Perception Subsystem.
-PLN006 | Planning.Inputs.Prediction.Predicted Trajectories | 1 | The Planning Module MAY have access to predicted trajectories of objects detected by the Perception Subsystem. | In simple planners, there is no prediction of moving objects, but in more complex planners, this may be considered. 
+PLN006 | Planning.Inputs.Prediction.Predicted Trajectories | 1 | The Planning Module MAY have access to predicted trajectories of objects detected by the Perception Subsystem. | In simple planners, there is no prediction of moving objects, but in more complex planners, this may be considered.
 PLN007 | Planning.Inputs.Localization.Current Pose | 1 | The Planning Module MUST have access to the robot's current pose. | The pose could be be provided manually or automatically determined (outside of this module).
 PLN008 | Planning.Outputs.Path | 1 | The Planning Module SHOULD output the Path for the robot to follow to execute the input Navigation Command and MUST respect any associated policy.
 PLN009 | Planning.Feedback.Inputs | 1 | The Planning Module MAY receive error input from the downstream Execution Module. | So that it can attempt to recover from execution failures.
@@ -303,7 +303,7 @@ PER002 | Perception.Latency | 1 | *TODO*
 
 ### 2.4.3 Prediction
 
-The Prediction Subsystem uses input from the Perception Subsystem and predicts the trajectories of the detected objects over time. 
+The Prediction Subsystem uses input from the Perception Subsystem and predicts the trajectories of the detected objects over time.
 
 Id | Handle | Priority | Description | Notes
 -- | ------ | -------- | ----------- | -----
@@ -325,8 +325,8 @@ LOC002 | Localization.Robot Pose.Accuracy | 1 | The Localization Module MUST pro
 * What are the scalability for the ROS2 Navigation System?
 * Any other important design goals to call out?
 * Should we specify a minimum target platform? Or, should this be expressed as minimum platform requirements?
-* What is the right latency value for detecting a collision? 
-* Should we add any safety-related functionality at the robot interface level? 
-* Do safety zones need unique names? 
+* What is the right latency value for detecting a collision?
+* Should we add any safety-related functionality at the robot interface level?
+* Do safety zones need unique names?
 * What is the target latency for the perception subsystem?
-* How far into the future should the object prediction work? 
+* How far into the future should the object prediction work?
diff --git a/doc/use_cases/README.md b/doc/use_cases/README.md
index 47d6ee56885..4b7ec8d64db 100644
--- a/doc/use_cases/README.md
+++ b/doc/use_cases/README.md
@@ -13,4 +13,3 @@ The Nav2 system is targeting the following use cases:
 
 ## Stretch Target
 3D Navigation - Drones
-
diff --git a/doc/use_cases/_template_use_case.md b/doc/use_cases/_template_use_case.md
index a7c3fcb3dd3..4c583a94b74 100644
--- a/doc/use_cases/_template_use_case.md
+++ b/doc/use_cases/_template_use_case.md
@@ -3,7 +3,7 @@ As a \<Developer, Researcher, Technician, etc.> I want the robot to \<action> so
 
 ## More details
 - Why is this needed?
-- What is the expected user interaction? 
+- What is the expected user interaction?
 - Are there any non-functional requirements? (build system, tools, performance, etc)
 
 # Example:
@@ -15,7 +15,7 @@ As a robot user, I want the robot to navigate without colliding into people or o
  - Why is this needed?
    - I want this so that I know the robot won't damage itself, damage property or hurt anyone
    - Example: a logistics robot in a warehouse must avoid shelves, people, forklifts, and other robots
- - What is the expected user interaction? 
+ - What is the expected user interaction?
    - I shouldn't have to interact with the robot to prevent it from crashing into people or things
 - Are there any non-functional requirements? (build system, tools, performance, etc)
    - The performance needs to be fast enough to avoid moving objects such as people walking or other moving robots
diff --git a/doc/use_cases/collision_avoidance_use_case.md b/doc/use_cases/collision_avoidance_use_case.md
index 3af0998c798..193067e01c1 100644
--- a/doc/use_cases/collision_avoidance_use_case.md
+++ b/doc/use_cases/collision_avoidance_use_case.md
@@ -10,5 +10,3 @@ As a Robot user I want to my robot to avoid colliding with people or objects so
    - The user should be able to walk in front of a robot and it should avoid crashing into that person
 
 - Are there any non-functional requirements? (build system, tools, performance, etc)
-
-
diff --git a/doc/use_cases/indoor_localization_use_case.md b/doc/use_cases/indoor_localization_use_case.md
index cee2149d89f..a0c9ab8dcdc 100644
--- a/doc/use_cases/indoor_localization_use_case.md
+++ b/doc/use_cases/indoor_localization_use_case.md
@@ -11,5 +11,3 @@ As a Robot user I want my robot to know its location on a given map of an indoor
    - The robot should be able to deduce it's own position on a map autonomously
 
 - Are there any non-functional requirements? (build system, tools, performance, etc)
-
-
diff --git a/doc/use_cases/indoor_navigation_use_case.md b/doc/use_cases/indoor_navigation_use_case.md
index 81a0824485f..5e6f954fb23 100644
--- a/doc/use_cases/indoor_navigation_use_case.md
+++ b/doc/use_cases/indoor_navigation_use_case.md
@@ -7,8 +7,6 @@ As a Robot user I want my robot to autonomously navigate to a given location on
    - Example: a courier robot in a logistics warehouse
 
 - What is the expected user interaction?
-   - The user should be able to specify a map to use and a location on that map for the robot to move to. 
+   - The user should be able to specify a map to use and a location on that map for the robot to move to.
 
 - Are there any non-functional requirements? (build system, tools, performance, etc)
-
-
diff --git a/doc/use_cases/keep_out_zones_use_case.md b/doc/use_cases/keep_out_zones_use_case.md
index 448d38267f9..967916e2ce2 100644
--- a/doc/use_cases/keep_out_zones_use_case.md
+++ b/doc/use_cases/keep_out_zones_use_case.md
@@ -1,5 +1,5 @@
 # Keep Out Zones
-As a Robot user I want to be able to designate keep-out zones or areas on a map so that my robot will go around those areas instead of through them 
+As a Robot user I want to be able to designate keep-out zones or areas on a map so that my robot will go around those areas instead of through them
 
 ## More details
 - Why is this needed?
@@ -10,5 +10,3 @@ As a Robot user I want to be able to designate keep-out zones or areas on a map
    - The user should be able to specify keep out zones for the robot to avoid
 
 - Are there any non-functional requirements? (build system, tools, performance, etc)
-
-
diff --git a/doc/use_cases/multi-story-building_use_case.md b/doc/use_cases/multi-story-building_use_case.md
index e86b3a77c68..6b6a3d6b2fc 100644
--- a/doc/use_cases/multi-story-building_use_case.md
+++ b/doc/use_cases/multi-story-building_use_case.md
@@ -3,13 +3,11 @@ As a Robot user I want my robot to be able to navigate stairways, ramps or eleva
 
 ## More details
 - Why is this needed?
-   - Example: a delivery robot in an office building 
+   - Example: a delivery robot in an office building
 
 - What is the expected user interaction?
    - The user should be able to specify stairways, ramps and elevators on a map for a robot to use or not use
-     - via a GUI 
+     - via a GUI
      - via a config file or API so that it can be done by another program
 
 - Are there any non-functional requirements? (build system, tools, performance, etc)
-
-
diff --git a/doc/use_cases/outdoor_localization_use_case.md b/doc/use_cases/outdoor_localization_use_case.md
index f9f9e926fc0..af8e83d1528 100644
--- a/doc/use_cases/outdoor_localization_use_case.md
+++ b/doc/use_cases/outdoor_localization_use_case.md
@@ -11,5 +11,3 @@ As a Robot user I want my robot to know its location on a given map of an outdoo
    - The robot should be able to deduce it's own position on a map autonomously
 
 - Are there any non-functional requirements? (build system, tools, performance, etc)
-
-
diff --git a/doc/use_cases/outdoor_navigation_use_case.md b/doc/use_cases/outdoor_navigation_use_case.md
index 8eb8da7cba5..050a8804337 100644
--- a/doc/use_cases/outdoor_navigation_use_case.md
+++ b/doc/use_cases/outdoor_navigation_use_case.md
@@ -7,8 +7,6 @@ As a Robot user I want my robot to autonomously navigate to a given location on
    - Example: a delivery robot on a college campus
 
 - What is the expected user interaction?
-   - The user should be able to specify a map to use and a location on that map for the robot to move to. 
+   - The user should be able to specify a map to use and a location on that map for the robot to move to.
 
 - Are there any non-functional requirements? (build system, tools, performance, etc)
-
-
diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
index 7928f5e9545..5910181b3a6 100644
--- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp
+++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
@@ -47,6 +47,7 @@
 #include "pluginlib/class_loader.hpp"
 #include "rclcpp/node_options.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
+#include "nav2_util/service_server.hpp"
 #include "std_srvs/srv/empty.hpp"
 #include "tf2_ros/message_filter.h"
 #include "tf2_ros/transform_broadcaster.h"
@@ -206,7 +207,8 @@ class AmclNode : public nav2_util::LifecycleNode
    * @brief Initialize state services
    */
   void initServices();
-  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr global_loc_srv_;
+  nav2_util::ServiceServer<std_srvs::srv::Empty,
+    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr global_loc_srv_;
   /*
    * @brief Service callback for a global relocalization request
    */
@@ -216,7 +218,8 @@ class AmclNode : public nav2_util::LifecycleNode
     std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
   // service server for providing an initial pose guess
-  rclcpp::Service<nav2_msgs::srv::SetInitialPose>::SharedPtr initial_guess_srv_;
+  nav2_util::ServiceServer<nav2_msgs::srv::SetInitialPose,
+    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr initial_guess_srv_;
   /*
    * @brief Service callback for an initial pose guess request
    */
@@ -226,7 +229,8 @@ class AmclNode : public nav2_util::LifecycleNode
     std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response> response);
 
   // Let amcl update samples without requiring motion
-  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr nomotion_update_srv_;
+  nav2_util::ServiceServer<std_srvs::srv::Empty,
+    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr nomotion_update_srv_;
   /*
    * @brief Request an AMCL update even though the robot hasn't moved
    */
diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
index 7ae2bb61048..eea641770e5 100644
--- a/nav2_amcl/src/amcl_node.cpp
+++ b/nav2_amcl/src/amcl_node.cpp
@@ -1564,19 +1564,25 @@ AmclNode::initPubSub()
 void
 AmclNode::initServices()
 {
-  global_loc_srv_ = create_service<std_srvs::srv::Empty>(
-    "reinitialize_global_localization",
-    std::bind(&AmclNode::globalLocalizationCallback, this, std::placeholders::_1,
+  global_loc_srv_ = std::make_shared<nav2_util::ServiceServer<std_srvs::srv::Empty,
+      std::shared_ptr<nav2_util::LifecycleNode>>>(
+    "reinitialize_global_localization", shared_from_this(),
+    std::bind(
+      &AmclNode::globalLocalizationCallback, this, std::placeholders::_1,
       std::placeholders::_2, std::placeholders::_3));
 
-  initial_guess_srv_ = create_service<nav2_msgs::srv::SetInitialPose>(
-    "set_initial_pose",
-    std::bind(&AmclNode::initialPoseReceivedSrv, this, std::placeholders::_1, std::placeholders::_2,
+  initial_guess_srv_ = std::make_shared<nav2_util::ServiceServer<nav2_msgs::srv::SetInitialPose,
+      std::shared_ptr<nav2_util::LifecycleNode>>>(
+    "set_initial_pose", shared_from_this(),
+    std::bind(
+      &AmclNode::initialPoseReceivedSrv, this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3));
 
-  nomotion_update_srv_ = create_service<std_srvs::srv::Empty>(
-    "request_nomotion_update",
-    std::bind(&AmclNode::nomotionUpdateCallback, this, std::placeholders::_1, std::placeholders::_2,
+  nomotion_update_srv_ = std::make_shared<nav2_util::ServiceServer<std_srvs::srv::Empty,
+      std::shared_ptr<nav2_util::LifecycleNode>>>(
+    "request_nomotion_update", shared_from_this(),
+    std::bind(
+      &AmclNode::nomotionUpdateCallback, this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3));
 }
 
diff --git a/nav2_amcl/src/map/map.c b/nav2_amcl/src/map/map.c
index c96c25772a2..a89c91242c0 100644
--- a/nav2_amcl/src/map/map.c
+++ b/nav2_amcl/src/map/map.c
@@ -49,6 +49,9 @@ map_t * map_alloc(void)
   // Allocate storage for main map
   map->cells = (map_cell_t *) NULL;
 
+  // Initialize max_occ_dist to 0.0
+  map->max_occ_dist = 0.0;
+
   return map;
 }
 
diff --git a/nav2_amcl/src/map/map_cspace.cpp b/nav2_amcl/src/map/map_cspace.cpp
index d6c55879508..7b179b9e0f7 100644
--- a/nav2_amcl/src/map/map_cspace.cpp
+++ b/nav2_amcl/src/map/map_cspace.cpp
@@ -25,15 +25,15 @@
 #include "nav2_amcl/map/map.hpp"
 
 /*
- * @class CellData
+ * @struct CellData
  * @brief Data about map cells
  */
-class CellData
+struct CellData
 {
-public:
   map_t * map_;
   unsigned int i_, j_;
   unsigned int src_i_, src_j_;
+  float occ_dist;
 };
 
 /*
@@ -82,9 +82,7 @@ class CachedDistanceMap
  */
 bool operator<(const CellData & a, const CellData & b)
 {
-  return a.map_->cells[MAP_INDEX(
-             a.map_, a.i_,
-             a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist;
+  return a.occ_dist > b.occ_dist;
 }
 
 /*
@@ -118,7 +116,9 @@ void enqueue(
   CachedDistanceMap * cdm,
   unsigned char * marked)
 {
-  if (marked[MAP_INDEX(map, i, j)]) {
+  const int map_index = MAP_INDEX(map, i, j);
+
+  if (marked[map_index]) {
     return;
   }
 
@@ -130,18 +130,14 @@ void enqueue(
     return;
   }
 
-  map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;
-
-  CellData cell;
-  cell.map_ = map;
-  cell.i_ = i;
-  cell.j_ = j;
-  cell.src_i_ = src_i;
-  cell.src_j_ = src_j;
+  map->cells[map_index].occ_dist = distance * map->scale;
 
-  Q.push(cell);
+  Q.emplace(
+    CellData{map, static_cast<unsigned int>(i), static_cast<unsigned int>(j),
+      static_cast<unsigned int>(src_i), static_cast<unsigned int>(src_j),
+      map->cells[map_index].occ_dist});
 
-  marked[MAP_INDEX(map, i, j)] = 1;
+  marked[map_index] = 1;
 }
 
 /*
@@ -164,16 +160,18 @@ void map_update_cspace(map_t * map, double max_occ_dist)
   // Enqueue all the obstacle cells
   CellData cell;
   cell.map_ = map;
+  int loop_map_index;
   for (int i = 0; i < map->size_x; i++) {
     cell.src_i_ = cell.i_ = i;
     for (int j = 0; j < map->size_y; j++) {
-      if (map->cells[MAP_INDEX(map, i, j)].occ_state == +1) {
-        map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
+      loop_map_index = MAP_INDEX(map, i, j);
+      if (map->cells[loop_map_index].occ_state == +1) {
+        map->cells[loop_map_index].occ_dist = 0.0;
         cell.src_j_ = cell.j_ = j;
-        marked[MAP_INDEX(map, i, j)] = 1;
+        marked[loop_map_index] = 1;
         Q.push(cell);
       } else {
-        map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
+        map->cells[loop_map_index].occ_dist = max_occ_dist;
       }
     }
   }
diff --git a/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp b/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp
index a8e3b7a1f63..2ded9407eb5 100644
--- a/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp
+++ b/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp
@@ -35,7 +35,15 @@ LikelihoodFieldModel::LikelihoodFieldModel(
   z_hit_ = z_hit;
   z_rand_ = z_rand;
   sigma_hit_ = sigma_hit;
-  map_update_cspace(map, max_occ_dist);
+
+  // recompute cspace only when necessary, i.e. if:
+  // - max_occ_dist changed
+  // OR
+  // - cspace was not computed yet, i.e. when map->max_occ_dist == 0.0 (and hence different from
+  // max_occ_dist)
+  if (map->max_occ_dist != max_occ_dist) {
+    map_update_cspace(map, max_occ_dist);
+  }
 }
 
 double
diff --git a/nav2_amcl/src/sensors/laser/likelihood_field_model_prob.cpp b/nav2_amcl/src/sensors/laser/likelihood_field_model_prob.cpp
index ce9dc0331e7..d40a80f8988 100644
--- a/nav2_amcl/src/sensors/laser/likelihood_field_model_prob.cpp
+++ b/nav2_amcl/src/sensors/laser/likelihood_field_model_prob.cpp
@@ -43,7 +43,15 @@ LikelihoodFieldModelProb::LikelihoodFieldModelProb(
   beam_skip_distance_ = beam_skip_distance;
   beam_skip_threshold_ = beam_skip_threshold;
   beam_skip_error_threshold_ = beam_skip_error_threshold;
-  map_update_cspace(map, max_occ_dist);
+
+  // recompute cspace only when necessary, i.e. if:
+  // - max_occ_dist changed
+  // OR
+  // - cspace was not computed yet, i.e. when map->max_occ_dist == 0.0 (and hence different from
+  // max_occ_dist)
+  if (map->max_occ_dist != max_occ_dist) {
+    map_update_cspace(map, max_occ_dist);
+  }
 }
 
 // Determine the probability for the given pose
diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md
index dab5b9a8a06..f0d34789c0e 100644
--- a/nav2_behavior_tree/README.md
+++ b/nav2_behavior_tree/README.md
@@ -9,7 +9,7 @@ The nav2_behavior_tree module provides:
 
 See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-bt-xml.html) for additional parameter descriptions and a list of XML nodes made available in this package. Also review the [Nav2 Behavior Tree Explanation](https://docs.nav2.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. A [tutorial](https://docs.nav2.org/plugin_tutorials/docs/writing_new_bt_plugin.html) is also provided to explain how to create a simple BT plugin.
 
-See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. 
+See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins.
 
 ## The bt_action_node Template and the Behavior Tree Engine
 
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp
index a0d86649c00..db5f938a9fe 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp
@@ -22,6 +22,7 @@
 
 #include "behaviortree_cpp/behavior_tree.h"
 #include "behaviortree_cpp/bt_factory.h"
+#include "behaviortree_cpp/loggers/groot2_publisher.h"
 #include "behaviortree_cpp/xml_parsing.h"
 
 #include "rclcpp/rclcpp.hpp"
@@ -85,6 +86,18 @@ class BehaviorTreeEngine
     const std::string & file_path,
     BT::Blackboard::Ptr blackboard);
 
+  /**
+   * @brief Add Groot2 monitor to publish BT status changes
+   * @param tree BT to monitor
+   * @param server_port Groot2 Server port, first of the pair (server_port, publisher_port)
+   */
+  void addGrootMonitoring(BT::Tree * tree, uint16_t server_port);
+
+  /**
+   * @brief Reset groot monitor
+   */
+  void resetGrootMonitor();
+
   /**
    * @brief Function to explicitly reset all BT nodes to initial state
    * @param tree Tree to halt
@@ -97,6 +110,9 @@ class BehaviorTreeEngine
 
   // Clock
   rclcpp::Clock::SharedPtr clock_;
+
+  // Groot2 monitor
+  std::unique_ptr<BT::Groot2Publisher> groot_monitor_;
 };
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
index 357903df7dc..32e65a7b5b7 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
@@ -20,9 +20,11 @@
 #include <chrono>
 
 #include "behaviortree_cpp/action_node.h"
+#include "behaviortree_cpp/json_export.h"
 #include "nav2_util/node_utils.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -32,6 +34,8 @@ using namespace std::chrono_literals;  // NOLINT
 /**
  * @brief Abstract class representing an action based BT node
  * @tparam ActionT Type of action
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 template<class ActionT>
 class BtActionNode : public BT::ActionNodeBase
@@ -389,7 +393,7 @@ class BtActionNode : public BT::ActionNodeBase
       };
     send_goal_options.feedback_callback =
       [this](typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr,
-      const std::shared_ptr<const typename ActionT::Feedback> feedback) {
+        const std::shared_ptr<const typename ActionT::Feedback> feedback) {
         feedback_ = feedback;
         emitWakeUpSignal();
       };
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
index b9cc3596e20..3ce2f816e88 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
@@ -91,6 +91,13 @@ class BtActionServer
    */
   bool on_cleanup();
 
+  /**
+   * @brief Enable (or disable) Groot2 monitoring of BT
+   * @param enable Groot2 monitoring
+   * @param server_port Groot2 Server port, first of the pair (server_port, publisher_port)
+   */
+  void setGrootMonitoring(const bool enable, const unsigned server_port);
+
   /**
    * @brief Replace current BT with another one
    * @param bt_xml_filename The file containing the new BT, uses default filename if empty
@@ -277,6 +284,10 @@ class BtActionServer
   // should the BT be reloaded even if the same xml filename is requested?
   bool always_reload_bt_xml_ = false;
 
+  // Parameters for Groot2 monitoring
+  bool enable_groot_monitoring_ = true;
+  int groot_server_port_ = 1667;
+
   // User-provided callbacks
   OnGoalReceivedCallback on_goal_received_callback_;
   OnLoopCallback on_loop_callback_;
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
index dbc124441e0..3f43b50e9b9 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -90,9 +90,10 @@ BtActionServer<ActionT>::BtActionServer(
   };
 
   if (node->has_parameter("error_code_names")) {
-    throw std::runtime_error("parameter 'error_code_names' has been replaced by "
-      " 'error_code_name_prefixes' and MUST be removed.\n"
-      " Please review migration guide and update your configuration.");
+    throw std::runtime_error(
+            "parameter 'error_code_names' has been replaced by "
+            " 'error_code_name_prefixes' and MUST be removed.\n"
+            " Please review migration guide and update your configuration.");
   }
 
   if (!node->has_parameter("error_code_name_prefixes")) {
@@ -118,8 +119,9 @@ BtActionServer<ActionT>::BtActionServer(
       for (const auto & error_code_name_prefix : error_code_name_prefixes) {
         error_code_name_prefixes_str += " " + error_code_name_prefix;
       }
-      RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:"
-        << error_code_name_prefixes_str);
+      RCLCPP_INFO_STREAM(
+        logger_, "Error_code parameters were set to:"
+          << error_code_name_prefixes_str);
     }
   }
 }
@@ -239,10 +241,18 @@ bool BtActionServer<ActionT>::on_cleanup()
   current_bt_xml_filename_.clear();
   blackboard_.reset();
   bt_->haltAllActions(tree_);
+  bt_->resetGrootMonitor();
   bt_.reset();
   return true;
 }
 
+template<class ActionT>
+void BtActionServer<ActionT>::setGrootMonitoring(const bool enable, const unsigned server_port)
+{
+  enable_groot_monitoring_ = enable;
+  groot_server_port_ = server_port;
+}
+
 template<class ActionT>
 bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filename)
 {
@@ -255,11 +265,15 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
     return true;
   }
 
+  // if a new tree is created, than the Groot2 Publisher must be destroyed
+  bt_->resetGrootMonitor();
+
   // Read the input BT XML from the specified file into a string
   std::ifstream xml_file(filename);
 
   if (!xml_file.good()) {
-    setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+    setInternalError(
+      ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
       "Couldn't open input XML file: " + filename);
     return false;
   }
@@ -277,7 +291,8 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
         wait_for_service_timeout_);
     }
   } catch (const std::exception & e) {
-    setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+    setInternalError(
+      ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
       std::string("Exception when loading BT: ") + e.what());
     return false;
   }
@@ -285,6 +300,15 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
   topic_logger_ = std::make_unique<RosTopicLogger>(client_node_, tree_);
 
   current_bt_xml_filename_ = filename;
+
+  // Enable monitoring with Groot2
+  if (enable_groot_monitoring_) {
+    bt_->addGrootMonitoring(&tree_, groot_server_port_);
+    RCLCPP_DEBUG(
+      logger_, "Enabling Groot2 monitoring for %s: %d",
+      action_name_.c_str(), groot_server_port_);
+  }
+
   return true;
 }
 
@@ -344,7 +368,8 @@ void BtActionServer<ActionT>::executeCallback()
 
     case nav2_behavior_tree::BtStatus::FAILED:
       action_server_->terminate_current(result);
-      RCLCPP_ERROR(logger_, "Goal failed error_code:%d error_msg:'%s'", result->error_code,
+      RCLCPP_ERROR(
+        logger_, "Goal failed error_code:%d error_msg:'%s'", result->error_code,
         result->error_msg.c_str());
       break;
 
@@ -362,7 +387,8 @@ void BtActionServer<ActionT>::setInternalError(uint16_t error_code, const std::s
 {
   internal_error_code_ = error_code;
   internal_error_msg_ = error_msg;
-  RCLCPP_ERROR(logger_, "Setting internal error error_code:%d, error_msg:%s",
+  RCLCPP_ERROR(
+    logger_, "Setting internal error error_code:%d, error_msg:%s",
     internal_error_code_, internal_error_msg_.c_str());
 }
 
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
index 18d6ea851bb..3b82ee24356 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
@@ -20,9 +20,12 @@
 #include <chrono>
 
 #include "behaviortree_cpp/action_node.h"
+#include "behaviortree_cpp/json_export.h"
 #include "nav2_util/node_utils.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
+#include "nav2_util/service_client.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -32,6 +35,8 @@ using namespace std::chrono_literals;  // NOLINT
 /**
  * @brief Abstract class representing a service based BT node
  * @tparam ServiceT Type of service
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 template<class ServiceT>
 class BtServiceNode : public BT::ActionNodeBase
@@ -109,16 +114,8 @@ class BtServiceNode : public BT::ActionNodeBase
     if (service_new != service_name_ || !service_client_) {
       service_name_ = service_new;
       node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
-      callback_group_ = node_->create_callback_group(
-        rclcpp::CallbackGroupType::MutuallyExclusive,
-        false);
-      callback_group_executor_.add_callback_group(callback_group_,
-          node_->get_node_base_interface());
-
-      service_client_ = node_->create_client<ServiceT>(
-        service_name_,
-        rclcpp::SystemDefaultsQoS(),
-        callback_group_);
+      service_client_ = std::make_shared<nav2_util::ServiceClient<ServiceT>>(
+        service_name_, node_, true /*creates and spins an internal executor*/);
     }
   }
 
@@ -173,7 +170,7 @@ class BtServiceNode : public BT::ActionNodeBase
         return BT::NodeStatus::FAILURE;
       }
 
-      future_result_ = service_client_->async_send_request(request_).share();
+      future_result_ = service_client_->async_call(request_);
       sent_time_ = node_->now();
       request_sent_ = true;
     }
@@ -221,7 +218,7 @@ class BtServiceNode : public BT::ActionNodeBase
       auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining;
 
       rclcpp::FutureReturnCode rc;
-      rc = callback_group_executor_.spin_until_future_complete(future_result_, timeout);
+      rc = service_client_->spin_until_complete(future_result_, timeout);
       if (rc == rclcpp::FutureReturnCode::SUCCESS) {
         request_sent_ = false;
         BT::NodeStatus status = on_completion(future_result_.get());
@@ -265,13 +262,11 @@ class BtServiceNode : public BT::ActionNodeBase
   }
 
   std::string service_name_, service_node_name_;
-  typename std::shared_ptr<rclcpp::Client<ServiceT>> service_client_;
+  typename nav2_util::ServiceClient<ServiceT>::SharedPtr service_client_;
   std::shared_ptr<typename ServiceT::Request> request_;
 
   // The node that will be used for any ROS operations
   rclcpp::Node::SharedPtr node_;
-  rclcpp::CallbackGroup::SharedPtr callback_group_;
-  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   // The timeout value while to use in the tick loop while waiting for
   // a result from the server
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp
index 63595fef985..ace8f07b527 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp
@@ -27,6 +27,7 @@
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "nav_msgs/msg/goals.hpp"
+#include "nav2_msgs/msg/waypoint_status.hpp"
 
 namespace BT
 {
@@ -43,6 +44,13 @@ namespace BT
 template<>
 inline geometry_msgs::msg::Point convertFromString(const StringView key)
 {
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<geometry_msgs::msg::Point>(new_key);
+  }
+
   // three real numbers separated by semicolons
   auto parts = BT::splitString(key, ';');
   if (parts.size() != 3) {
@@ -64,6 +72,13 @@ inline geometry_msgs::msg::Point convertFromString(const StringView key)
 template<>
 inline geometry_msgs::msg::Quaternion convertFromString(const StringView key)
 {
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<geometry_msgs::msg::Quaternion>(new_key);
+  }
+
   // four real numbers separated by semicolons
   auto parts = BT::splitString(key, ';');
   if (parts.size() != 4) {
@@ -86,6 +101,13 @@ inline geometry_msgs::msg::Quaternion convertFromString(const StringView key)
 template<>
 inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key)
 {
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<geometry_msgs::msg::PoseStamped>(new_key);
+  }
+
   // 7 real numbers separated by semicolons
   auto parts = BT::splitString(key, ';');
   if (parts.size() != 9) {
@@ -113,6 +135,13 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key)
 template<>
 inline std::vector<geometry_msgs::msg::PoseStamped> convertFromString(const StringView key)
 {
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<std::vector<geometry_msgs::msg::PoseStamped>>(new_key);
+  }
+
   auto parts = BT::splitString(key, ';');
   if (parts.size() % 9 != 0) {
     throw std::runtime_error("invalid number of fields for std::vector<PoseStamped> attribute)");
@@ -143,6 +172,13 @@ inline std::vector<geometry_msgs::msg::PoseStamped> convertFromString(const Stri
 template<>
 inline nav_msgs::msg::Goals convertFromString(const StringView key)
 {
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<nav_msgs::msg::Goals>(new_key);
+  }
+
   auto parts = BT::splitString(key, ';');
   if ((parts.size() - 2) % 9 != 0) {
     throw std::runtime_error("invalid number of fields for Goals attribute)");
@@ -175,6 +211,13 @@ inline nav_msgs::msg::Goals convertFromString(const StringView key)
 template<>
 inline nav_msgs::msg::Path convertFromString(const StringView key)
 {
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<nav_msgs::msg::Path>(new_key);
+  }
+
   auto parts = BT::splitString(key, ';');
   if ((parts.size() - 2) % 9 != 0) {
     throw std::runtime_error("invalid number of fields for Path attribute)");
@@ -200,32 +243,101 @@ inline nav_msgs::msg::Path convertFromString(const StringView key)
 }
 
 /**
- * @brief Parse XML string to std::chrono::milliseconds
+ * @brief Parse XML string to nav2_msgs::msg::WaypointStatus
  * @param key XML string
- * @return std::chrono::milliseconds
+ * @return nav2_msgs::msg::WaypointStatus
  */
 template<>
-inline std::chrono::milliseconds convertFromString<std::chrono::milliseconds>(const StringView key)
+inline nav2_msgs::msg::WaypointStatus convertFromString(const StringView key)
 {
-  return std::chrono::milliseconds(std::stoul(key.data()));
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<nav2_msgs::msg::WaypointStatus>(new_key);
+  }
+
+  auto parts = BT::splitString(key, ';');
+  if (parts.size() != 13) {
+    throw std::runtime_error("invalid number of fields for WaypointStatus attribute)");
+  } else {
+    nav2_msgs::msg::WaypointStatus waypoint_status;
+    waypoint_status.waypoint_status = BT::convertFromString<uint8_t>(parts[0]);
+    waypoint_status.waypoint_index = BT::convertFromString<uint32_t>(parts[1]);
+    waypoint_status.waypoint_pose.header.stamp =
+      rclcpp::Time(BT::convertFromString<int64_t>(parts[2]));
+    waypoint_status.waypoint_pose.header.frame_id = BT::convertFromString<std::string>(parts[3]);
+    waypoint_status.waypoint_pose.pose.position.x = BT::convertFromString<double>(parts[4]);
+    waypoint_status.waypoint_pose.pose.position.y = BT::convertFromString<double>(parts[5]);
+    waypoint_status.waypoint_pose.pose.position.z = BT::convertFromString<double>(parts[6]);
+    waypoint_status.waypoint_pose.pose.orientation.x = BT::convertFromString<double>(parts[7]);
+    waypoint_status.waypoint_pose.pose.orientation.y = BT::convertFromString<double>(parts[8]);
+    waypoint_status.waypoint_pose.pose.orientation.z = BT::convertFromString<double>(parts[9]);
+    waypoint_status.waypoint_pose.pose.orientation.w = BT::convertFromString<double>(parts[10]);
+    waypoint_status.error_code = BT::convertFromString<uint16_t>(parts[11]);
+    waypoint_status.error_msg = BT::convertFromString<std::string>(parts[12]);
+    return waypoint_status;
+  }
 }
 
 /**
- * @brief Parse XML string to std::set<int>
+ * @brief Parse XML string to nav2_msgs::msg::WaypointStatus
  * @param key XML string
- * @return std::set<int>
+ * @return nav2_msgs::msg::WaypointStatus
  */
 template<>
-inline std::set<int> convertFromString(StringView key)
+inline std::vector<nav2_msgs::msg::WaypointStatus> convertFromString(const StringView key)
 {
-  // Real numbers separated by semicolons
-  auto parts = splitString(key, ';');
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<std::vector<nav2_msgs::msg::WaypointStatus>>(new_key);
+  }
 
-  std::set<int> set;
-  for (const auto part : parts) {
-    set.insert(convertFromString<int>(part));
+  auto parts = BT::splitString(key, ';');
+  if (parts.size() % 13 != 0) {
+    throw std::runtime_error("invalid number of fields for std::vector<WaypointStatus> attribute)");
+  } else {
+    std::vector<nav2_msgs::msg::WaypointStatus> wp_status_vector;
+    for (size_t i = 0; i < parts.size(); i += 13) {
+      nav2_msgs::msg::WaypointStatus wp_status;
+      wp_status.waypoint_status = BT::convertFromString<uint8_t>(parts[i]);
+      wp_status.waypoint_index = BT::convertFromString<uint32_t>(parts[i + 1]);
+      wp_status.waypoint_pose.header.stamp =
+        rclcpp::Time(BT::convertFromString<int64_t>(parts[i + 2]));
+      wp_status.waypoint_pose.header.frame_id = BT::convertFromString<std::string>(parts[i + 3]);
+      wp_status.waypoint_pose.pose.position.x = BT::convertFromString<double>(parts[i + 4]);
+      wp_status.waypoint_pose.pose.position.y = BT::convertFromString<double>(parts[i + 5]);
+      wp_status.waypoint_pose.pose.position.z = BT::convertFromString<double>(parts[i + 6]);
+      wp_status.waypoint_pose.pose.orientation.x = BT::convertFromString<double>(parts[i + 7]);
+      wp_status.waypoint_pose.pose.orientation.y = BT::convertFromString<double>(parts[i + 8]);
+      wp_status.waypoint_pose.pose.orientation.z = BT::convertFromString<double>(parts[i + 9]);
+      wp_status.waypoint_pose.pose.orientation.w = BT::convertFromString<double>(parts[i + 10]);
+      wp_status.error_code = BT::convertFromString<uint16_t>(parts[i + 11]);
+      wp_status.error_msg = BT::convertFromString<std::string>(parts[i + 12]);
+      wp_status_vector.push_back(wp_status);
+    }
+    return wp_status_vector;
   }
-  return set;
+}
+
+/**
+ * @brief Parse XML string to std::chrono::milliseconds
+ * @param key XML string
+ * @return std::chrono::milliseconds
+ */
+template<>
+inline std::chrono::milliseconds convertFromString<std::chrono::milliseconds>(const StringView key)
+{
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<std::chrono::milliseconds>(new_key);
+  }
+
+  return std::chrono::milliseconds(std::stoul(key.data()));
 }
 
 /**
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/json_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/json_utils.hpp
new file mode 100644
index 00000000000..db43a42752d
--- /dev/null
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/json_utils.hpp
@@ -0,0 +1,141 @@
+// Copyright (c) 2025 Alberto J. Tudela Roldán
+// Copyright (c) 2025 Grupo Avispa, DTE, Universidad de Málaga
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__JSON_UTILS_HPP_
+#define NAV2_BEHAVIOR_TREE__JSON_UTILS_HPP_
+
+#include <string>
+#include <set>
+#include <vector>
+
+#include "rclcpp/time.hpp"
+#include "rclcpp/node.hpp"
+#include "behaviortree_cpp/json_export.h"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+#include "nav_msgs/msg/goals.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_msgs/msg/waypoint_status.hpp"
+
+// The follow templates are required when using Groot 2 to visualize the BT. They
+// convert the data types into JSON format easy for visualization.
+
+namespace builtin_interfaces::msg
+{
+
+BT_JSON_CONVERTER(builtin_interfaces::msg::Time, msg)
+{
+  add_field("sec", &msg.sec);
+  add_field("nanosec", &msg.nanosec);
+}
+
+}  // namespace builtin_interfaces::msg
+
+namespace std_msgs::msg
+{
+
+BT_JSON_CONVERTER(std_msgs::msg::Header, msg)
+{
+  add_field("stamp", &msg.stamp);
+  add_field("frame_id", &msg.frame_id);
+}
+
+}  // namespace std_msgs::msg
+
+namespace geometry_msgs::msg
+{
+
+BT_JSON_CONVERTER(geometry_msgs::msg::Point, msg)
+{
+  add_field("x", &msg.x);
+  add_field("y", &msg.y);
+  add_field("z", &msg.z);
+}
+
+BT_JSON_CONVERTER(geometry_msgs::msg::Quaternion, msg)
+{
+  add_field("x", &msg.x);
+  add_field("y", &msg.y);
+  add_field("z", &msg.z);
+  add_field("w", &msg.w);
+}
+
+BT_JSON_CONVERTER(geometry_msgs::msg::Pose, msg)
+{
+  add_field("position", &msg.position);
+  add_field("orientation", &msg.orientation);
+}
+
+BT_JSON_CONVERTER(geometry_msgs::msg::PoseStamped, msg)
+{
+  add_field("header", &msg.header);
+  add_field("pose", &msg.pose);
+}
+
+}  // namespace geometry_msgs::msg
+
+namespace nav_msgs::msg
+{
+
+BT_JSON_CONVERTER(nav_msgs::msg::Goals, msg)
+{
+  add_field("header", &msg.header);
+  add_field("goals", &msg.goals);
+}
+
+BT_JSON_CONVERTER(nav_msgs::msg::Path, msg)
+{
+  add_field("header", &msg.header);
+  add_field("poses", &msg.poses);
+}
+
+}  // namespace nav_msgs::msg
+
+namespace nav2_msgs::msg
+{
+
+BT_JSON_CONVERTER(nav2_msgs::msg::WaypointStatus, msg)
+{
+  add_field("waypoint_status", &msg.waypoint_status);
+  add_field("waypoint_index", &msg.waypoint_index);
+  add_field("waypoint_pose", &msg.waypoint_pose);
+  add_field("error_code", &msg.error_code);
+  add_field("error_msg", &msg.error_msg);
+}
+
+}  // namespace nav2_msgs::msg
+
+namespace std
+{
+
+inline void from_json(const nlohmann::json & js, std::chrono::milliseconds & dest)
+{
+  if (js.contains("ms")) {
+    dest = std::chrono::milliseconds(js.at("ms").get<int>());
+  } else {
+    throw std::runtime_error("Invalid JSON for std::chrono::milliseconds");
+  }
+}
+
+inline void to_json(nlohmann::json & js, const std::chrono::milliseconds & src)
+{
+  js["__type"] = "std::chrono::milliseconds";
+  js["ms"] = src.count();
+}
+
+}  // namespace std
+
+#endif  // NAV2_BEHAVIOR_TREE__JSON_UTILS_HPP_
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp
index 097b1ccbb3f..f3f7e0794c0 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp
@@ -26,6 +26,8 @@ namespace nav2_behavior_tree
 
 /**
  * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::AssistedTeleop
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTeleop>
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp
index b3a6c9490a4..85d9a9246b5 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp
@@ -25,6 +25,8 @@ namespace nav2_behavior_tree
 
 /**
  * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp
index 0bb8ad0bb27..3b3e00b2ca4 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp
@@ -27,6 +27,8 @@ namespace nav2_behavior_tree
 
 /**
  * @brief A nav2_behavior_tree::BtServiceNode class that wraps nav2_msgs::srv::ClearEntireCostmap
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>
 {
@@ -50,6 +52,8 @@ class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEnti
 /**
  * @brief A nav2_behavior_tree::BtServiceNode class that
  * wraps nav2_msgs::srv::ClearCostmapExceptRegion
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class ClearCostmapExceptRegionService
   : public BtServiceNode<nav2_msgs::srv::ClearCostmapExceptRegion>
@@ -88,6 +92,8 @@ class ClearCostmapExceptRegionService
 /**
  * @brief A nav2_behavior_tree::BtServiceNode class that
  * wraps nav2_msgs::srv::ClearCostmapAroundRobot
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class ClearCostmapAroundRobotService : public BtServiceNode<nav2_msgs::srv::ClearCostmapAroundRobot>
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp
index 17b2e1af987..8fb2844ab7b 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp
@@ -28,6 +28,8 @@ namespace nav2_behavior_tree
 
 /**
  * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputePathThroughPoses
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class ComputePathThroughPosesAction
   : public BtActionNode<nav2_msgs::action::ComputePathThroughPoses>
@@ -78,6 +80,10 @@ class ComputePathThroughPosesAction
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<nav_msgs::msg::Path>();
+    BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
+
     return providedBasicPorts(
       {
         BT::InputPort<nav_msgs::msg::Goals>(
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp
index a356f4a6221..1d1f352b1d3 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp
@@ -26,6 +26,8 @@ namespace nav2_behavior_tree
 
 /**
  * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputePathToPose
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePathToPose>
 {
@@ -75,11 +77,19 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<nav_msgs::msg::Path>();
+    BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
+
     return providedBasicPorts(
       {
         BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
         BT::InputPort<geometry_msgs::msg::PoseStamped>(
-          "start", "Start pose of the path if overriding current robot pose"),
+          "start",
+          "Used as the planner start pose instead of the current robot pose, if use_start is"
+          " not false (i.e. not provided or set to true)"),
+        BT::InputPort<bool>(
+          "use_start", "For using or not using (i.e. ignoring) the provided start pose"),
         BT::InputPort<std::string>(
           "planner_id", "",
           "Mapped name to the planner plugin type to use"),
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp
index 0b3546600e3..44ac5ce40c0 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp
@@ -34,6 +34,7 @@ namespace nav2_behavior_tree
  * to get the decision about what controller must be used. It is usually used before of
  * the FollowPath. The selected_controller output port is passed to controller_id
  * input port of the FollowPath
+ * @note This is an Asynchronous node. It will re-initialize when halted.
  */
 class ControllerSelector : public BT::SyncActionNode
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp
index 3fb5a56fb8c..55a30e74c3b 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp
@@ -25,6 +25,8 @@ namespace nav2_behavior_tree
 
 /**
  * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::DriveOnHeading
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeading>
 {
@@ -86,9 +88,6 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
    * @brief Function to perform some user-defined operation upon cancellation of the action
    */
   BT::NodeStatus on_cancelled() override;
-
-private:
-  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
index 6716faa8fba..14cf510d776 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
@@ -26,6 +26,8 @@ namespace nav2_behavior_tree
 
 /**
  * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::FollowPath
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
 {
@@ -78,6 +80,9 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<nav_msgs::msg::Path>();
+
     return providedBasicPorts(
       {
         BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp
index 091360b4795..fa336182064 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp
@@ -19,11 +19,15 @@
 #include <memory>
 #include <string>
 
+#include "behaviortree_cpp/action_node.h"
+#include "behaviortree_cpp/json_export.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/path.h"
+#include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
-#include "behaviortree_cpp/action_node.h"
-#include "nav_msgs/msg/path.h"
+
 
 namespace nav2_behavior_tree
 {
@@ -38,6 +42,10 @@ class GetPoseFromPath : public BT::ActionNodeBase
 
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
+    BT::RegisterJsonDefinition<nav_msgs::msg::Path>();
+
     return {
       BT::InputPort<nav_msgs::msg::Path>("path", "Path to extract pose from"),
       BT::OutputPort<geometry_msgs::msg::PoseStamped>("pose", "Stamped Extracted Pose"),
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp
index 24b15ee5730..16ae0b96596 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp
@@ -34,6 +34,7 @@ namespace nav2_behavior_tree
  * to get the decision about what goal_checker must be used. It is usually used before of
  * the FollowPath. The selected_goal_checker output port is passed to goal_checker_id
  * input port of the FollowPath
+ * @note This is an Asynchronous node. It will re-initialize when halted.
  */
 class GoalCheckerSelector : public BT::SyncActionNode
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp
index 8d636d84e45..fa175002a15 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp
@@ -27,6 +27,8 @@ namespace nav2_behavior_tree
 
 /**
  * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::NavigateThroughPoses
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::NavigateThroughPoses>
 {
@@ -71,6 +73,9 @@ class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::Naviga
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<nav_msgs::msg::Goals>();
+
     return providedBasicPorts(
       {
         BT::InputPort<nav_msgs::msg::Goals>(
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp
index ff917561c27..e277f90aa22 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp
@@ -27,6 +27,8 @@ namespace nav2_behavior_tree
 
 /**
  * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::NavigateToPose
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPose>
 {
@@ -71,6 +73,9 @@ class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPo
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
+
     return providedBasicPorts(
       {
         BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp
index ec11584e9b2..173b2cf7b92 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp
@@ -34,6 +34,7 @@ namespace nav2_behavior_tree
  * to get the decision about what planner must be used. It is usually used before of
  * the ComputePathToPoseAction. The selected_planner output port is passed to planner_id
  * input port of the ComputePathToPoseAction
+ * @note This is an Asynchronous node. It will re-initialize when halted.
  */
 class PlannerSelector : public BT::SyncActionNode
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp
index 912c932d762..2b910404ba3 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp
@@ -33,6 +33,7 @@ namespace nav2_behavior_tree
  * to get the decision about what progress_checker must be used. It is usually used before of
  * the FollowPath. The selected_progress_checker output port is passed to progress_checker_id
  * input port of the FollowPath
+ * @note This is an Asynchronous node. It will re-initialize when halted.
  */
 class ProgressCheckerSelector : public BT::SyncActionNode
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp
index 8d90327f201..cf358cb937c 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp
@@ -25,6 +25,8 @@ namespace nav2_behavior_tree
 
 /**
  * @brief A nav2_behavior_tree::BtServiceNode class that wraps nav2_msgs::srv::Empty
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class ReinitializeGlobalLocalizationService : public BtServiceNode<std_srvs::srv::Empty>
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp
index 7d3a4615bdb..87682156c32 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp
@@ -23,10 +23,17 @@
 #include "nav_msgs/msg/goals.hpp"
 #include "nav2_behavior_tree/bt_service_node.hpp"
 #include "nav2_msgs/srv/get_costs.hpp"
+#include "nav2_msgs/msg/waypoint_status.hpp"
 
 namespace nav2_behavior_tree
 {
 
+/**
+ * @brief A nav2_behavior_tree::BtServiceNode class that removes goals that are in collision in on the global costmap
+ *        wraps nav2_msgs::srv::GetCosts
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
+ */
 class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
 {
 public:
@@ -50,9 +57,15 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
 
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<nav_msgs::msg::Goals>();
+    BT::RegisterJsonDefinition<nav2_msgs::msg::WaypointStatus>();
+    BT::RegisterJsonDefinition<std::vector<nav2_msgs::msg::WaypointStatus>>();
+
     return providedBasicPorts(
       {
-        BT::InputPort<nav_msgs::msg::Goals>("input_goals",
+        BT::InputPort<nav_msgs::msg::Goals>(
+          "input_goals",
           "Original goals to remove from"),
         BT::InputPort<double>(
           "cost_threshold", 254.0,
@@ -61,8 +74,15 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
         BT::InputPort<bool>(
           "consider_unknown_as_obstacle", false,
           "Whether to consider unknown cost as obstacle"),
-        BT::OutputPort<nav_msgs::msg::Goals>("output_goals",
+        BT::OutputPort<nav_msgs::msg::Goals>(
+          "output_goals",
           "Goals with in-collision goals removed"),
+        BT::InputPort<std::vector<nav2_msgs::msg::WaypointStatus>>(
+          "input_waypoint_statuses",
+          "Original waypoint_statuses to mark waypoint status from"),
+        BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>(
+          "output_waypoint_statuses",
+          "Waypoint_statuses with in-collision waypoints marked")
       });
   }
 
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp
index 4a17896f89c..d8fe0e9b834 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp
@@ -19,15 +19,23 @@
 #include <memory>
 #include <string>
 
+#include "behaviortree_cpp/action_node.h"
+#include "behaviortree_cpp/json_export.h"
 #include "nav_msgs/msg/goals.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
+#include "nav2_msgs/msg/waypoint_status.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
-#include "behaviortree_cpp/action_node.h"
-#include "nav2_behavior_tree/bt_utils.hpp"
+
 
 namespace nav2_behavior_tree
 {
 
+/**
+ * @brief A BT::ActionNodeBase that removes goals that the robot passed near to
+ * @note This is an Asynchronous node. It will re-initialize when halted.
+ */
 class RemovePassedGoals : public BT::ActionNodeBase
 {
 public:
@@ -42,13 +50,26 @@ class RemovePassedGoals : public BT::ActionNodeBase
 
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<nav_msgs::msg::Goals>();
+    BT::RegisterJsonDefinition<nav2_msgs::msg::WaypointStatus>();
+    BT::RegisterJsonDefinition<std::vector<nav2_msgs::msg::WaypointStatus>>();
+
     return {
-      BT::InputPort<nav_msgs::msg::Goals>("input_goals",
-          "Original goals to remove viapoints from"),
-      BT::OutputPort<nav_msgs::msg::Goals>("output_goals",
-          "Goals with passed viapoints removed"),
+      BT::InputPort<nav_msgs::msg::Goals>(
+        "input_goals",
+        "Original goals to remove viapoints from"),
+      BT::OutputPort<nav_msgs::msg::Goals>(
+        "output_goals",
+        "Goals with passed viapoints removed"),
       BT::InputPort<double>("radius", 0.5, "radius to goal for it to be considered for removal"),
       BT::InputPort<std::string>("robot_base_frame", "Robot base frame"),
+      BT::InputPort<std::vector<nav2_msgs::msg::WaypointStatus>>(
+        "input_waypoint_statuses",
+        "Original waypoint_statuses to mark waypoint status from"),
+      BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>(
+        "output_waypoint_statuses",
+        "Waypoint_statuses with passed waypoints marked")
     };
   }
 
@@ -58,6 +79,7 @@ class RemovePassedGoals : public BT::ActionNodeBase
 
   double viapoint_achieved_radius_;
   double transform_tolerance_;
+  rclcpp::Node::SharedPtr node_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::string robot_base_frame_;
 };
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp
index 499c0558548..4ca9ddebab9 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp
@@ -71,6 +71,9 @@ class SmoothPathAction : public nav2_behavior_tree::BtActionNode<nav2_msgs::acti
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<nav_msgs::msg::Path>();
+
     return providedBasicPorts(
       {
         BT::InputPort<nav_msgs::msg::Path>("unsmoothed_path", "Path to be smoothed"),
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp
index 254457b438e..2dd743d1af5 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp
@@ -34,6 +34,7 @@ namespace nav2_behavior_tree
  * to get the decision about what smoother must be used. It is usually used before of
  * the FollowPath. The selected_smoother output port is passed to smoother_id
  * input port of the FollowPath
+ * @note This is an Asynchronous node. It will re-initialize when halted.
  */
 class SmootherSelector : public BT::SyncActionNode
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp
index e6a4b4cef0e..32ef354498b 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp
@@ -25,6 +25,8 @@ namespace nav2_behavior_tree
 
 /**
  * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Spin
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp
index 5501d49cf32..25009266f42 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp
@@ -19,9 +19,12 @@
 #include <memory>
 #include <string>
 
+#include "behaviortree_cpp/action_node.h"
+#include "behaviortree_cpp/json_export.h"
 #include "nav_msgs/msg/path.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
 
-#include "behaviortree_cpp/action_node.h"
 
 namespace nav2_behavior_tree
 {
@@ -47,6 +50,9 @@ class TruncatePath : public BT::ActionNodeBase
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<nav_msgs::msg::Path>();
+
     return {
       BT::InputPort<nav_msgs::msg::Path>("input_path", "Original Path"),
       BT::OutputPort<nav_msgs::msg::Path>("output_path", "Path truncated to a certain distance"),
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp
index 94b4b66ca2a..c8affd1ccf5 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp
@@ -20,11 +20,14 @@
 #include <string>
 #include <limits>
 
-#include "nav_msgs/msg/path.hpp"
-
 #include "behaviortree_cpp/action_node.h"
+#include "behaviortree_cpp/json_export.h"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
 #include "tf2_ros/buffer.h"
 
+
 namespace nav2_behavior_tree
 {
 
@@ -49,6 +52,10 @@ class TruncatePathLocal : public BT::ActionNodeBase
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<nav_msgs::msg::Path>();
+    BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
+
     return {
       BT::InputPort<nav_msgs::msg::Path>("input_path", "Original Path"),
       BT::OutputPort<nav_msgs::msg::Path>(
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp
index f224d3e8c04..f4e65a7bf82 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp
@@ -25,6 +25,8 @@ namespace nav2_behavior_tree
 
 /**
  * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp
index dd200f318b3..e0837126b1f 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp
@@ -32,6 +32,8 @@ namespace nav2_behavior_tree
 /**
  * @brief A BT::ConditionNode that returns SUCCESS every time the robot
  * travels a specified distance and FAILURE otherwise
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class DistanceTraveledCondition : public BT::ConditionNode
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp
index 3c5e9cc5f9c..7d71bd097bd 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp
@@ -21,8 +21,10 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "behaviortree_cpp/condition_node.h"
+#include "behaviortree_cpp/json_export.h"
 #include "nav_msgs/msg/goals.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
 
 
 namespace nav2_behavior_tree
@@ -58,6 +60,10 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
+    BT::RegisterJsonDefinition<nav_msgs::msg::Goals>();
+
     return {
       BT::InputPort<nav_msgs::msg::Goals>(
         "goals", "Vector of navigation goals"),
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp
index 2771e657b9a..fa4b26e33e6 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp
@@ -20,8 +20,11 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "behaviortree_cpp/condition_node.h"
-#include "tf2_ros/buffer.h"
+#include "behaviortree_cpp/json_export.h"
 #include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
+#include "tf2_ros/buffer.h"
+
 
 namespace nav2_behavior_tree
 {
@@ -29,6 +32,8 @@ namespace nav2_behavior_tree
 /**
  * @brief A BT::ConditionNode that returns SUCCESS when a specified goal
  * is reached and FAILURE otherwise
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class GoalReachedCondition : public BT::ConditionNode
 {
@@ -72,6 +77,9 @@ class GoalReachedCondition : public BT::ConditionNode
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
+
     return {
       BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination"),
       BT::InputPort<std::string>("robot_base_frame", "Robot base frame")
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp
index 7f37723a2b3..4f9b0aa831c 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp
@@ -19,8 +19,11 @@
 #include <vector>
 
 #include "behaviortree_cpp/condition_node.h"
+#include "behaviortree_cpp/json_export.h"
 #include "nav_msgs/msg/goals.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
+
 
 namespace nav2_behavior_tree
 {
@@ -28,6 +31,8 @@ namespace nav2_behavior_tree
 /**
  * @brief A BT::ConditionNode that returns SUCCESS when goal is
  * updated on the blackboard and FAILURE otherwise
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class GoalUpdatedCondition : public BT::ConditionNode
 {
@@ -55,6 +60,10 @@ class GoalUpdatedCondition : public BT::ConditionNode
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
+    BT::RegisterJsonDefinition<nav_msgs::msg::Goals>();
+
     return {
       BT::InputPort<nav_msgs::msg::Goals>(
         "goals", "Vector of navigation goals"),
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp
index 0af8ca24b29..41702b48751 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp
@@ -29,6 +29,8 @@ namespace nav2_behavior_tree
 /**
  * @brief A BT::ConditionNode that listens to a battery topic and
  * returns SUCCESS when battery is charging and FAILURE otherwise
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class IsBatteryChargingCondition : public BT::ConditionNode
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp
index 188f820e454..17ac78b2edc 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp
@@ -30,6 +30,8 @@ namespace nav2_behavior_tree
 /**
  * @brief A BT::ConditionNode that listens to a battery topic and
  * returns SUCCESS when battery is low and FAILURE otherwise
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class IsBatteryLowCondition : public BT::ConditionNode
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp
index 5acc938a28f..09d0ba36c13 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp
@@ -20,8 +20,13 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "behaviortree_cpp/condition_node.h"
+#include "behaviortree_cpp/json_export.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_msgs/srv/is_path_valid.hpp"
+#include "nav2_util/service_client.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
+
 
 namespace nav2_behavior_tree
 {
@@ -61,19 +66,23 @@ class IsPathValidCondition : public BT::ConditionNode
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<nav_msgs::msg::Path>();
+    BT::RegisterJsonDefinition<std::chrono::milliseconds>();
+
     return {
       BT::InputPort<nav_msgs::msg::Path>("path", "Path to Check"),
       BT::InputPort<std::chrono::milliseconds>("server_timeout"),
       BT::InputPort<unsigned int>("max_cost", 253, "Maximum cost of the path"),
       BT::InputPort<bool>(
-          "consider_unknown_as_obstacle", false,
-          "Whether to consider unknown cost as obstacle")
+        "consider_unknown_as_obstacle", false,
+        "Whether to consider unknown cost as obstacle")
     };
   }
 
 private:
   rclcpp::Node::SharedPtr node_;
-  rclcpp::Client<nav2_msgs::srv::IsPathValid>::SharedPtr client_;
+  nav2_util::ServiceClient<nav2_msgs::srv::IsPathValid>::SharedPtr client_;
   // The timeout value while waiting for a response from the
   // is path valid service
   std::chrono::milliseconds server_timeout_;
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp
index 40197939d1f..a5eab6bfda8 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp
@@ -16,15 +16,15 @@
 #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STOPPED_CONDITION_HPP_
 
 #include <string>
-#include <atomic>
-#include <deque>
 #include <memory>
 
 #include "rclcpp/rclcpp.hpp"
 #include "behaviortree_cpp/condition_node.h"
+#include "behaviortree_cpp/json_export.h"
 #include "nav_msgs/msg/odometry.hpp"
 #include "nav2_util/odometry_utils.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
 
 
 using namespace std::chrono_literals; // NOLINT
@@ -67,11 +67,16 @@ class IsStoppedCondition : public BT::ConditionNode
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<std::chrono::milliseconds>();
+
     return {
-      BT::InputPort<double>("velocity_threshold", 0.01,
-          "Velocity threshold below which robot is considered stopped"),
-      BT::InputPort<std::chrono::milliseconds>("duration_stopped", 1000ms,
-          "Duration (ms) the velocity must remain below the threshold"),
+      BT::InputPort<double>(
+        "velocity_threshold", 0.01,
+        "Velocity threshold below which robot is considered stopped"),
+      BT::InputPort<std::chrono::milliseconds>(
+        "duration_stopped", 1000ms,
+        "Duration (ms) the velocity must remain below the threshold"),
     };
   }
 
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp
index 5e770b4bbd6..0460cdf3080 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp
@@ -19,8 +19,10 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "behaviortree_cpp/condition_node.h"
+#include "behaviortree_cpp/json_export.h"
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -55,6 +57,9 @@ class PathExpiringTimerCondition : public BT::ConditionNode
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<nav_msgs::msg::Path>();
+
     return {
       BT::InputPort<double>("seconds", 1.0, "Seconds"),
       BT::InputPort<nav_msgs::msg::Path>("path")
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp
index 2e34689c56a..0ea482ab4f2 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp
@@ -27,6 +27,8 @@ namespace nav2_behavior_tree
 /**
  * @brief A BT::ConditionNode that returns SUCCESS every time a specified
  * time period passes and FAILURE otherwise
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class TimeExpiredCondition : public BT::ConditionNode
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp
index 732e862ff4c..13d61a74a6e 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp
@@ -29,6 +29,8 @@ namespace nav2_behavior_tree
 /**
  * @brief A BT::ConditionNode that returns SUCCESS if there is a valid transform
  * between two specified frames and FAILURE otherwise
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class TransformAvailableCondition : public BT::ConditionNode
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp
index 38cf3369b0d..e649fd75984 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp
@@ -31,6 +31,8 @@ namespace nav2_behavior_tree
 /**
  * @brief A BT::DecoratorNode that ticks its child every time the robot
  * travels a specified distance
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class DistanceController : public BT::DecoratorNode
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp
index f24a7ff3d76..c72b6470910 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp
@@ -20,16 +20,20 @@
 #include <vector>
 
 #include "behaviortree_cpp/decorator_node.h"
-
-#include "rclcpp/rclcpp.hpp"
+#include "behaviortree_cpp/json_export.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
+#include "rclcpp/rclcpp.hpp"
+
 
 namespace nav2_behavior_tree
 {
 
 /**
  * @brief A BT::DecoratorNode that ticks its child if the goal was updated
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class GoalUpdatedController : public BT::DecoratorNode
 {
@@ -49,6 +53,10 @@ class GoalUpdatedController : public BT::DecoratorNode
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
+    BT::RegisterJsonDefinition<nav_msgs::msg::Goals>();
+
     return {
       BT::InputPort<nav_msgs::msg::Goals>(
         "goals", "Vector of navigation goals"),
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp
index 71bcf8f5598..6a8ce0911d6 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp
@@ -20,19 +20,23 @@
 #include <memory>
 #include <string>
 
+#include "behaviortree_cpp/decorator_node.h"
+#include "behaviortree_cpp/json_export.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav_msgs/msg/goals.hpp"
-
-#include "behaviortree_cpp/decorator_node.h"
-
+#include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+
 namespace nav2_behavior_tree
 {
 
 /**
  * @brief A BT::DecoratorNode that subscribes to a goal topic and updates
  * the current goal on the blackboard
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class GoalUpdater : public BT::DecoratorNode
 {
@@ -52,13 +56,19 @@ class GoalUpdater : public BT::DecoratorNode
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
+    BT::RegisterJsonDefinition<nav_msgs::msg::Goals>();
+
     return {
       BT::InputPort<geometry_msgs::msg::PoseStamped>("input_goal", "Original Goal"),
       BT::InputPort<nav_msgs::msg::Goals>("input_goals", "Original Goals"),
-      BT::OutputPort<geometry_msgs::msg::PoseStamped>("output_goal",
-          "Received Goal by subscription"),
-      BT::OutputPort<nav_msgs::msg::Goals>("output_goals",
-          "Received Goals by subscription")
+      BT::OutputPort<geometry_msgs::msg::PoseStamped>(
+        "output_goal",
+        "Received Goal by subscription"),
+      BT::OutputPort<nav_msgs::msg::Goals>(
+        "output_goals",
+        "Received Goals by subscription")
     };
   }
 
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp
index 8420927d30c..a679cf6dc76 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp
@@ -19,9 +19,12 @@
 #include <memory>
 #include <limits>
 
+#include "behaviortree_cpp/decorator_node.h"
+#include "behaviortree_cpp/json_export.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav_msgs/msg/path.hpp"
-#include "behaviortree_cpp/decorator_node.h"
+#include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 namespace nav2_behavior_tree
@@ -49,6 +52,9 @@ class PathLongerOnApproach : public BT::DecoratorNode
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<nav_msgs::msg::Path>();
+
     return {
       BT::InputPort<nav_msgs::msg::Path>("path", "Planned Path"),
       BT::InputPort<double>(
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp
index ccc346eb16d..194a057e312 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp
@@ -25,6 +25,8 @@ namespace nav2_behavior_tree
 
 /**
  * @brief A BT::DecoratorNode that ticks its child at a specified rate
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class RateController : public BT::DecoratorNode
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp
index 02c0c1812ef..7e6de3529e7 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp
@@ -25,7 +25,9 @@ namespace nav2_behavior_tree
 
 /**
  * @brief A BT::DecoratorNode that triggers its child only once and returns FAILURE
- * for every succeeding tick
+ * for every succeeding tick (reset when halted)
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class SingleTrigger : public BT::DecoratorNode
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp
index 632a941acbe..3afc019af5b 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp
@@ -21,11 +21,13 @@
 #include <vector>
 #include <deque>
 
+#include "behaviortree_cpp/decorator_node.h"
+#include "behaviortree_cpp/json_export.h"
 #include "nav_msgs/msg/odometry.hpp"
 #include "nav2_util/odometry_utils.hpp"
-
-#include "behaviortree_cpp/decorator_node.h"
 #include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
+
 
 namespace nav2_behavior_tree
 {
@@ -34,6 +36,8 @@ namespace nav2_behavior_tree
  * @brief A BT::DecoratorNode that ticks its child every at a rate proportional to
  * the speed of the robot. If the robot travels faster, this node will tick its child at a
  * higher frequency and reduce the tick frequency if the robot slows down
+ * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
+ *       It will re-initialize when halted.
  */
 class SpeedController : public BT::DecoratorNode
 {
@@ -53,6 +57,10 @@ class SpeedController : public BT::DecoratorNode
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
+    BT::RegisterJsonDefinition<nav_msgs::msg::Goals>();
+
     return {
       BT::InputPort<double>("min_rate", 0.1, "Minimum rate"),
       BT::InputPort<double>("max_rate", 1.0, "Maximum rate"),
diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
index 585272e849e..a7cea1fb6bd 100644
--- a/nav2_behavior_tree/nav2_tree_nodes.xml
+++ b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -78,7 +78,8 @@
 
     <Action ID="ComputePathToPose">
       <input_port name="goal">Destination to plan to</input_port>
-      <input_port name="start">Start pose of the path if overriding current robot pose</input_port>
+      <input_port name="start">Use as the planner start pose instead of the current robot pose, if use_start is not false (i.e. not provided or set to true)</input_port>
+      <input_port name="use_start">For using or not using (i.e. ignoring) the provided start pose</input_port>
       <input_port name="planner_id">Mapped name to the planner plugin type to use</input_port>
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
@@ -102,7 +103,9 @@
       <input_port name="input_goals">Input goals to remove if passed</input_port>
       <input_port name="radius">Radius tolerance on a goal to consider it passed</input_port>
       <input_port name="robot_base_frame">Robot base frame</input_port>
+      <input_port name="input_waypoint_statuses">Original waypoint_statuses to mark waypoint status from</input_port>
       <output_port name="output_goals">Set of goals after removing any passed</output_port>
+      <output_port name="output_waypoint_statuses">Waypoint_statuses with passed waypoints marked</output_port>
     </Action>
 
     <Action ID="RemoveInCollisionGoals">
@@ -111,7 +114,9 @@
       <input_port name="use_footprint">Whether to use the footprint cost or the point cost.</input_port>
       <input_port name="cost_threshold">The cost threshold above which a waypoint is considered in collision and should be removed.</input_port>
       <input_port name="consider_unknown_as_obstacle"> If unknown cost is considered valid </input_port>
+      <input_port name="input_waypoint_statuses">Original waypoint_statuses to mark waypoint status from</input_port>
       <output_port name="output_goals">A vector of goals containing only those that are not in collision.</output_port>
+      <output_port name="output_waypoint_statuses">Waypoint_statuses with in-collision waypoints marked</output_port>
     </Action>
 
     <Action ID="SmoothPath">
diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
index c563cd1c4d9..ba02e8196d0 100644
--- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
+++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
@@ -32,8 +32,25 @@ void ComputePathToPoseAction::on_tick()
 {
   getInput("goal", goal_.goal);
   getInput("planner_id", goal_.planner_id);
-  if (getInput("start", goal_.start)) {
-    goal_.use_start = true;
+
+  // if "use_start" is provided try to enforce it (true or false), but we cannot enforce true if
+  // start is not provided
+  goal_.use_start = false;
+  if (getInput("use_start", goal_.use_start)) {
+    if (goal_.use_start && !getInput("start", goal_.start)) {
+      // in case we don't have a "start" pose
+      goal_.use_start = false;
+      RCLCPP_ERROR(
+        node_->get_logger(),
+        "use_start is set to true but no start pose was provided, falling back to default "
+        "behavior, i.e. using the current robot pose");
+    }
+  } else {
+    // else if "use_start" is not provided, but "start" is, then use it in order to not change
+    // the legacy behavior
+    if (getInput("start", goal_.start)) {
+      goal_.use_start = true;
+    }
   }
 }
 
diff --git a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp
index 8518a1a818f..f1730870b4b 100644
--- a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp
+++ b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp
@@ -33,6 +33,9 @@ ControllerSelector::ControllerSelector(
 : BT::SyncActionNode(name, conf)
 {
   initialize();
+
+  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
+  callback_group_executor_.spin_some(std::chrono::nanoseconds(1));
 }
 
 void ControllerSelector::initialize()
diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp
index 39a68e4c378..e3a0fc02765 100644
--- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp
+++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp
@@ -24,8 +24,7 @@ DriveOnHeadingAction::DriveOnHeadingAction(
   const std::string & xml_tag_name,
   const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: BtActionNode<nav2_msgs::action::DriveOnHeading>(xml_tag_name, action_name, conf),
-  initialized_(false)
+: BtActionNode<nav2_msgs::action::DriveOnHeading>(xml_tag_name, action_name, conf)
 {
 }
 
@@ -47,12 +46,11 @@ void DriveOnHeadingAction::initialize()
   goal_.speed = speed;
   goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
   goal_.disable_collision_checks = disable_collision_checks;
-  initialized_ = true;
 }
 
 void DriveOnHeadingAction::on_tick()
 {
-  if (!initialized_) {
+  if (!BT::isStatusActive(status())) {
     initialize();
   }
 }
diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp
index 081f36caee0..875c1b71c44 100644
--- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp
+++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp
@@ -66,7 +66,7 @@ void FollowPathAction::on_wait_for_result(
   getInput("path", new_path);
 
   // Check if it is not same with the current one
-  if (goal_.path != new_path) {
+  if (goal_.path != new_path && new_path != nav_msgs::msg::Path()) {
     // the action server on the next loop iteration
     goal_.path = new_path;
     goal_updated_ = true;
diff --git a/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp
index cab0311a37a..ce042d1f3be 100644
--- a/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp
+++ b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp
@@ -46,12 +46,12 @@ inline BT::NodeStatus GetPoseFromPath::tick()
   }
 
   // Account for negative indices
-  if(pose_index < 0) {
+  if (pose_index < 0) {
     pose_index = input_path.poses.size() + pose_index;
   }
 
   // out of bounds index
-  if(pose_index < 0 || static_cast<unsigned>(pose_index) >= input_path.poses.size()) {
+  if (pose_index < 0 || static_cast<unsigned>(pose_index) >= input_path.poses.size()) {
     return BT::NodeStatus::FAILURE;
   }
 
@@ -60,7 +60,7 @@ inline BT::NodeStatus GetPoseFromPath::tick()
   output_pose = input_path.poses[pose_index];
 
   // populate pose frame from path if necessary
-  if(output_pose.header.frame_id.empty()) {
+  if (output_pose.header.frame_id.empty()) {
     output_pose.header.frame_id = input_path.header.frame_id;
   }
 
diff --git a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp
index e25e2694fdf..4e9c1d1587d 100644
--- a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp
+++ b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp
@@ -33,6 +33,9 @@ PlannerSelector::PlannerSelector(
 : BT::SyncActionNode(name, conf)
 {
   initialize();
+
+  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
+  callback_group_executor_.spin_some(std::chrono::nanoseconds(1));
 }
 
 void PlannerSelector::initialize()
diff --git a/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp
index 3f4ea97c2d9..9d8d1b504c2 100644
--- a/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp
+++ b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp
@@ -52,7 +52,7 @@ void ProgressCheckerSelector::createROSInterfaces()
 
     progress_checker_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
       topic_name_, qos,
-        std::bind(&ProgressCheckerSelector::callbackProgressCheckerSelect, this, _1));
+      std::bind(&ProgressCheckerSelector::callbackProgressCheckerSelect, this, _1));
   }
 }
 
diff --git a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp
index 68a83771cbe..113c3a2f514 100644
--- a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp
+++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp
@@ -20,6 +20,7 @@
 #include "nav2_behavior_tree/bt_utils.hpp"
 #include "tf2/utils.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_util/geometry_utils.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -63,12 +64,29 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion(
     return BT::NodeStatus::FAILURE;
   }
 
+  // get the `waypoint_statuses` vector
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses;
+  auto waypoint_statuses_get_res = getInput("input_waypoint_statuses", waypoint_statuses);
+  if (!waypoint_statuses_get_res) {
+    RCLCPP_ERROR(node_->get_logger(), "Missing [input_waypoint_statuses] port input!");
+  }
+
   nav_msgs::msg::Goals valid_goal_poses;
   for (size_t i = 0; i < response->costs.size(); ++i) {
     if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) ||
       response->costs[i] < cost_threshold_)
     {
       valid_goal_poses.goals.push_back(input_goals_.goals[i]);
+    } else if (waypoint_statuses_get_res) {
+      using namespace nav2_util::geometry_utils;  // NOLINT
+      auto cur_waypoint_index =
+        find_next_matching_goal_in_waypoint_statuses(waypoint_statuses, input_goals_.goals[i]);
+      if (cur_waypoint_index == -1) {
+        RCLCPP_ERROR(node_->get_logger(), "Failed to find matching goal in waypoint_statuses");
+        return BT::NodeStatus::FAILURE;
+      }
+      waypoint_statuses[cur_waypoint_index].waypoint_status =
+        nav2_msgs::msg::WaypointStatus::SKIPPED;
     }
   }
   // Inform if all goals have been removed
@@ -78,6 +96,9 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion(
       "All goals are in collision and have been removed from the list");
   }
   setOutput("output_goals", valid_goal_poses);
+  // set `waypoint_statuses` output
+  setOutput("output_waypoint_statuses", waypoint_statuses);
+
   return BT::NodeStatus::SUCCESS;
 }
 
diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp
index b3cc3a39ead..8e5bec221bf 100644
--- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp
+++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp
@@ -36,11 +36,11 @@ void RemovePassedGoals::initialize()
   getInput("radius", viapoint_achieved_radius_);
 
   tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
-  auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
-  node->get_parameter("transform_tolerance", transform_tolerance_);
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  node_->get_parameter("transform_tolerance", transform_tolerance_);
 
   robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string>(
-    node, "robot_base_frame", this);
+    node_, "robot_base_frame", this);
 }
 
 inline BT::NodeStatus RemovePassedGoals::tick()
@@ -67,6 +67,13 @@ inline BT::NodeStatus RemovePassedGoals::tick()
     return BT::NodeStatus::FAILURE;
   }
 
+  // get the `waypoint_statuses` vector
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses;
+  auto waypoint_statuses_get_res = getInput("input_waypoint_statuses", waypoint_statuses);
+  if (!waypoint_statuses_get_res) {
+    RCLCPP_ERROR(node_->get_logger(), "Missing [input_waypoint_statuses] port input!");
+  }
+
   double dist_to_goal;
   while (goal_poses.goals.size() > 1) {
     dist_to_goal = euclidean_distance(goal_poses.goals[0].pose, current_pose.pose);
@@ -75,10 +82,24 @@ inline BT::NodeStatus RemovePassedGoals::tick()
       break;
     }
 
+    // mark waypoint statuses before the goal is erased from goals
+    if (waypoint_statuses_get_res) {
+      auto cur_waypoint_index =
+        find_next_matching_goal_in_waypoint_statuses(waypoint_statuses, goal_poses.goals[0]);
+      if (cur_waypoint_index == -1) {
+        RCLCPP_ERROR(node_->get_logger(), "Failed to find matching goal in waypoint_statuses");
+        return BT::NodeStatus::FAILURE;
+      }
+      waypoint_statuses[cur_waypoint_index].waypoint_status =
+        nav2_msgs::msg::WaypointStatus::COMPLETED;
+    }
+
     goal_poses.goals.erase(goal_poses.goals.begin());
   }
 
   setOutput("output_goals", goal_poses);
+  // set `waypoint_statuses` output
+  setOutput("output_waypoint_statuses", waypoint_statuses);
 
   return BT::NodeStatus::SUCCESS;
 }
diff --git a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp
index 254d45f8258..58dad4866db 100644
--- a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp
+++ b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp
@@ -34,6 +34,9 @@ SmootherSelector::SmootherSelector(
 : BT::SyncActionNode(name, conf)
 {
   initialize();
+
+  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
+  callback_group_executor_.spin_some(std::chrono::nanoseconds(1));
 }
 
 void SmootherSelector::initialize()
diff --git a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp
index 4d364744412..2135e86bf6a 100644
--- a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp
+++ b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp
@@ -27,6 +27,9 @@ IsBatteryChargingCondition::IsBatteryChargingCondition(
   is_battery_charging_(false)
 {
   initialize();
+
+  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
+  callback_group_executor_.spin_some(std::chrono::nanoseconds(1));
 }
 
 void IsBatteryChargingCondition::initialize()
diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp
index db1710027a9..16130224f59 100644
--- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp
+++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp
@@ -30,6 +30,9 @@ IsBatteryLowCondition::IsBatteryLowCondition(
   is_battery_low_(false)
 {
   initialize();
+
+  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
+  callback_group_executor_.spin_some(std::chrono::nanoseconds(1));
 }
 
 void IsBatteryLowCondition::initialize()
diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp
index 8caed0c3971..3aa63c74a7e 100644
--- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp
+++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp
@@ -27,7 +27,9 @@ IsPathValidCondition::IsPathValidCondition(
   max_cost_(253), consider_unknown_as_obstacle_(false)
 {
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
-  client_ = node_->create_client<nav2_msgs::srv::IsPathValid>("is_path_valid");
+  client_ = std::make_shared<nav2_util::ServiceClient<nav2_msgs::srv::IsPathValid>>(
+    "is_path_valid",
+    node_, false /* Does not create and spin an internal executor*/);
 
   server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
 }
@@ -53,14 +55,9 @@ BT::NodeStatus IsPathValidCondition::tick()
   request->path = path;
   request->max_cost = max_cost_;
   request->consider_unknown_as_obstacle = consider_unknown_as_obstacle_;
-  auto result = client_->async_send_request(request);
-
-  if (rclcpp::spin_until_future_complete(node_, result, server_timeout_) ==
-    rclcpp::FutureReturnCode::SUCCESS)
-  {
-    if (result.get()->is_valid) {
-      return BT::NodeStatus::SUCCESS;
-    }
+  auto response = client_->invoke(request, server_timeout_);
+  if (response->is_valid) {
+    return BT::NodeStatus::SUCCESS;
   }
   return BT::NodeStatus::FAILURE;
 }
diff --git a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp
index 23fd614cb7c..43535565002 100644
--- a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp
+++ b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp
@@ -49,7 +49,7 @@ BT::NodeStatus TimeExpiredCondition::tick()
     return BT::NodeStatus::FAILURE;
   }
 
-  // Determine how long its been since we've started this iteration
+  // Determine how long it's been since we've started this iteration
   auto elapsed = node_->now() - start_;
 
   // Now, get that in seconds
diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp
index 6eb3c6e99e5..942d869e37b 100644
--- a/nav2_behavior_tree/plugins/control/recovery_node.cpp
+++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp
@@ -52,6 +52,8 @@ BT::NodeStatus RecoveryNode::tick()
 
         case BT::NodeStatus::SUCCESS:
           // reset node and return success when first child returns success
+          // also halt the recovery action as the main action is successful, reset its state
+          ControlNode::haltChild(1);
           halt();
           return BT::NodeStatus::SUCCESS;
 
diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp
index de969937704..3bb1ee38c34 100644
--- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp
+++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp
@@ -36,6 +36,9 @@ GoalUpdater::GoalUpdater(
   goals_updater_topic_("goals_update")
 {
   initialize();
+
+  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
+  callback_group_executor_.spin_all(std::chrono::milliseconds(1));
 }
 
 void GoalUpdater::initialize()
@@ -54,7 +57,8 @@ void GoalUpdater::createROSInterfaces()
   std::string goal_updater_topic_new;
   std::string goals_updater_topic_new;
   node_->get_parameter_or<std::string>("goal_updater_topic", goal_updater_topic_new, "goal_update");
-  node_->get_parameter_or<std::string>("goals_updater_topic", goals_updater_topic_new,
+  node_->get_parameter_or<std::string>(
+    "goals_updater_topic", goals_updater_topic_new,
     "goals_update");
 
   // Only create a new subscriber if the topic has changed or subscriber is empty
@@ -92,15 +96,13 @@ inline BT::NodeStatus GoalUpdater::tick()
   getInput("input_goal", goal);
   getInput("input_goals", goals);
 
-  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
-  callback_group_executor_.spin_all(std::chrono::milliseconds(1));
   callback_group_executor_.spin_all(std::chrono::milliseconds(49));
 
   if (last_goal_received_set_) {
     if (last_goal_received_.header.stamp == rclcpp::Time(0)) {
-        // if the goal doesn't have a timestamp, we reject it
+      // if the goal doesn't have a timestamp, we reject it
       RCLCPP_WARN(
-          node_->get_logger(), "The received goal has no timestamp. Ignoring.");
+        node_->get_logger(), "The received goal has no timestamp. Ignoring.");
       setOutput("output_goal", goal);
     } else {
       auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp);
@@ -109,9 +111,9 @@ inline BT::NodeStatus GoalUpdater::tick()
         setOutput("output_goal", last_goal_received_);
       } else {
         RCLCPP_INFO(
-            node_->get_logger(), "The timestamp of the received goal (%f) is older than the "
-            "current goal (%f). Ignoring the received goal.",
-            last_goal_received_time.seconds(), goal_time.seconds());
+          node_->get_logger(), "The timestamp of the received goal (%f) is older than the "
+          "current goal (%f). Ignoring the received goal.",
+          last_goal_received_time.seconds(), goal_time.seconds());
         setOutput("output_goal", goal);
       }
     }
@@ -124,7 +126,7 @@ inline BT::NodeStatus GoalUpdater::tick()
       setOutput("output_goals", goals);
     } else if (last_goals_received_.header.stamp == rclcpp::Time(0)) {
       RCLCPP_WARN(
-          node_->get_logger(), "The received goals array has no timestamp. Ignoring.");
+        node_->get_logger(), "The received goals array has no timestamp. Ignoring.");
       setOutput("output_goals", goals);
     } else {
       auto last_goals_received_time = rclcpp::Time(last_goals_received_.header.stamp);
@@ -133,9 +135,9 @@ inline BT::NodeStatus GoalUpdater::tick()
         setOutput("output_goals", last_goals_received_);
       } else {
         RCLCPP_INFO(
-            node_->get_logger(), "The timestamp of the received goals (%f) is older than the "
-            "current goals (%f). Ignoring the received goals.",
-            last_goals_received_time.seconds(), goals_time.seconds());
+          node_->get_logger(), "The timestamp of the received goals (%f) is older than the "
+          "current goals (%f). Ignoring the received goals.",
+          last_goals_received_time.seconds(), goals_time.seconds());
         setOutput("output_goals", goals);
       }
     }
diff --git a/nav2_behavior_tree/plugins_list.hpp.in b/nav2_behavior_tree/plugins_list.hpp.in
index 148583b9271..40ffed01ed5 100644
--- a/nav2_behavior_tree/plugins_list.hpp.in
+++ b/nav2_behavior_tree/plugins_list.hpp.in
@@ -1,6 +1,6 @@
 
 // This was automativally generated by cmake
-namespace nav2::details 
+namespace nav2::details
 {
   const char* BT_BUILTIN_PLUGINS = "@plugin_libs@";
 }
diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp
index 7e8dc0a09cc..782a9660a8c 100644
--- a/nav2_behavior_tree/src/behavior_tree_engine.cpp
+++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp
@@ -20,7 +20,9 @@
 #include <vector>
 
 #include "rclcpp/rclcpp.hpp"
+#include "behaviortree_cpp/json_export.h"
 #include "behaviortree_cpp/utils/shared_library.h"
+#include "nav2_behavior_tree/json_utils.hpp"
 #include "nav2_behavior_tree/utils/loop_rate.hpp"
 
 namespace nav2_behavior_tree
@@ -99,6 +101,27 @@ BehaviorTreeEngine::createTreeFromFile(
   return factory_.createTreeFromFile(file_path, blackboard);
 }
 
+void
+BehaviorTreeEngine::addGrootMonitoring(
+  BT::Tree * tree,
+  uint16_t server_port)
+{
+  // This logger publish status changes using Groot2
+  groot_monitor_ = std::make_unique<BT::Groot2Publisher>(*tree, server_port);
+
+  // Register common types JSON definitions
+  BT::RegisterJsonDefinition<builtin_interfaces::msg::Time>();
+  BT::RegisterJsonDefinition<std_msgs::msg::Header>();
+}
+
+void
+BehaviorTreeEngine::resetGrootMonitor()
+{
+  if (groot_monitor_) {
+    groot_monitor_.reset();
+  }
+}
+
 // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state
 void
 BehaviorTreeEngine::haltAllActions(BT::Tree & tree)
diff --git a/nav2_behavior_tree/test/CMakeLists.txt b/nav2_behavior_tree/test/CMakeLists.txt
index 1d958ae4927..96676453327 100644
--- a/nav2_behavior_tree/test/CMakeLists.txt
+++ b/nav2_behavior_tree/test/CMakeLists.txt
@@ -4,6 +4,12 @@ target_link_libraries(test_bt_utils
   ${geometry_msgs_TARGETS}
 )
 
+ament_add_gtest(test_json_utils test_json_utils.cpp)
+target_link_libraries(test_json_utils
+  ${library_name}
+  ${geometry_msgs_TARGETS}
+)
+
 function(plugin_add_test target filename plugin)
   ament_add_gtest(${target} ${filename})
   target_link_libraries(${target}
diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp
index 4a475b0fd8c..dfd3eabe86e 100644
--- a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp
@@ -181,6 +181,141 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_su
   EXPECT_EQ(output_poses.goals[2], poses.goals[2]);
 }
 
+TEST_F(
+  RemoveInCollisionGoalsTestFixture,
+  test_tick_remove_in_collision_goals_success_and_output_waypoint_statues)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+          <RemoveInCollisionGoals service_name="/global_costmap/get_cost_global_costmap"
+                                  input_goals="{goals}" output_goals="{goals}"
+                                  cost_threshold="253"
+                                  input_waypoint_statuses="{waypoint_statuses}"
+                                  output_waypoint_statuses="{waypoint_statuses}"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new goal and set it on blackboard
+  nav_msgs::msg::Goals poses;
+  poses.goals.resize(4);
+  poses.goals[0].pose.position.x = 0.0;
+  poses.goals[0].pose.position.y = 0.0;
+
+  poses.goals[1].pose.position.x = 0.5;
+  poses.goals[1].pose.position.y = 0.0;
+
+  poses.goals[2].pose.position.x = 1.0;
+  poses.goals[2].pose.position.y = 0.0;
+
+  poses.goals[3].pose.position.x = 2.0;
+  poses.goals[3].pose.position.y = 0.0;
+
+  config_->blackboard->set("goals", poses);
+
+  // create waypoint_statuses and set it on blackboard
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses(poses.goals.size());
+  for (size_t i = 0; i < waypoint_statuses.size(); ++i) {
+    waypoint_statuses[i].waypoint_pose = poses.goals[i];
+    waypoint_statuses[i].waypoint_index = i;
+  }
+  config_->blackboard->set("waypoint_statuses", waypoint_statuses);
+
+  // tick until node is not running
+  tree_->rootNode()->executeTick();
+  while (tree_->rootNode()->status() == BT::NodeStatus::RUNNING) {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  // check that it removed the point in range
+  nav_msgs::msg::Goals output_poses;
+  EXPECT_TRUE(config_->blackboard->get("goals", output_poses));
+
+  EXPECT_EQ(output_poses.goals.size(), 3u);
+  EXPECT_EQ(output_poses.goals[0], poses.goals[0]);
+  EXPECT_EQ(output_poses.goals[1], poses.goals[1]);
+  EXPECT_EQ(output_poses.goals[2], poses.goals[2]);
+
+  // check the waypoint_statuses
+  std::vector<nav2_msgs::msg::WaypointStatus> output_waypoint_statuses;
+  EXPECT_TRUE(config_->blackboard->get("waypoint_statuses", output_waypoint_statuses));
+  EXPECT_EQ(output_waypoint_statuses.size(), 4u);
+  EXPECT_EQ(output_waypoint_statuses[0].waypoint_status, nav2_msgs::msg::WaypointStatus::PENDING);
+  EXPECT_EQ(output_waypoint_statuses[1].waypoint_status, nav2_msgs::msg::WaypointStatus::PENDING);
+  EXPECT_EQ(output_waypoint_statuses[2].waypoint_status, nav2_msgs::msg::WaypointStatus::PENDING);
+  EXPECT_EQ(output_waypoint_statuses[3].waypoint_status, nav2_msgs::msg::WaypointStatus::SKIPPED);
+}
+
+TEST_F(
+  RemoveInCollisionGoalsTestFixture,
+  test_tick_remove_in_collision_goals_find_matching_waypoint_fail)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+          <RemoveInCollisionGoals service_name="/global_costmap/get_cost_global_costmap"
+                                  input_goals="{goals}" output_goals="{goals}"
+                                  cost_threshold="253"
+                                  input_waypoint_statuses="{waypoint_statuses}"
+                                  output_waypoint_statuses="{waypoint_statuses}"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new goal and set it on blackboard
+  nav_msgs::msg::Goals poses;
+  poses.goals.resize(4);
+  poses.goals[0].pose.position.x = 0.0;
+  poses.goals[0].pose.position.y = 0.0;
+
+  poses.goals[1].pose.position.x = 0.5;
+  poses.goals[1].pose.position.y = 0.0;
+
+  poses.goals[2].pose.position.x = 1.0;
+  poses.goals[2].pose.position.y = 0.0;
+
+  poses.goals[3].pose.position.x = 2.0;
+  poses.goals[3].pose.position.y = 0.0;
+
+  config_->blackboard->set("goals", poses);
+
+  // create waypoint_statuses and set it on blackboard
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses(poses.goals.size());
+  for (size_t i = 0; i < waypoint_statuses.size(); ++i) {
+    waypoint_statuses[i].waypoint_pose = poses.goals[i];
+    waypoint_statuses[i].waypoint_index = i;
+  }
+  // inconsistency between waypoint_statuses and poses
+  waypoint_statuses[3].waypoint_pose.pose.position.x = 0.0;
+
+  config_->blackboard->set("waypoint_statuses", waypoint_statuses);
+
+  // tick until node is not running
+  tree_->rootNode()->executeTick();
+  while (tree_->rootNode()->status() == BT::NodeStatus::RUNNING) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check that it failed and returned the original goals
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
+  nav_msgs::msg::Goals output_poses;
+  EXPECT_TRUE(config_->blackboard->get("goals", output_poses));
+
+  EXPECT_EQ(output_poses.goals.size(), 4u);
+  EXPECT_EQ(output_poses.goals[0], poses.goals[0]);
+  EXPECT_EQ(output_poses.goals[1], poses.goals[1]);
+  EXPECT_EQ(output_poses.goals[2], poses.goals[2]);
+  EXPECT_EQ(output_poses.goals[3], poses.goals[3]);
+}
+
 TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fail)
 {
   // create tree
diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp
index 74fd7c6fdad..63cc1e5ec01 100644
--- a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp
@@ -39,6 +39,7 @@ class RemovePassedGoalsTestFixture : public ::testing::Test
 
     config_ = new BT::NodeConfiguration();
     transform_handler_ = std::make_shared<nav2_behavior_tree::TransformHandler>(node_);
+    transform_handler_->activate();
 
     // Create the blackboard that will be shared by all of the nodes in the tree
     config_->blackboard = BT::Blackboard::create();
@@ -98,7 +99,6 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick)
   pose.position.x = 0.25;
   pose.position.y = 0.0;
 
-  transform_handler_->activate();
   transform_handler_->waitForTransform();
   transform_handler_->updateRobotPose(pose);
 
@@ -144,6 +144,139 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick)
   EXPECT_EQ(output_poses.goals[1], poses.goals[3]);
 }
 
+TEST_F(
+  RemovePassedGoalsTestFixture,
+  test_tick_remove_passed_goals_success_and_output_waypoint_statuses)
+{
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = 0.25;
+  pose.position.y = 0.0;
+
+  transform_handler_->waitForTransform();
+  transform_handler_->updateRobotPose(pose);
+
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+          <RemovePassedGoals radius="0.5" input_goals="{goals}" output_goals="{goals}"
+                             input_waypoint_statuses="{waypoint_statuses}"
+                             output_waypoint_statuses="{waypoint_statuses}"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new goal and set it on blackboard
+  nav_msgs::msg::Goals poses;
+  poses.goals.resize(4);
+  poses.goals[0].pose.position.x = 0.0;
+  poses.goals[0].pose.position.y = 0.0;
+
+  poses.goals[1].pose.position.x = 0.5;
+  poses.goals[1].pose.position.y = 0.0;
+
+  poses.goals[2].pose.position.x = 1.0;
+  poses.goals[2].pose.position.y = 0.0;
+
+  poses.goals[3].pose.position.x = 2.0;
+  poses.goals[3].pose.position.y = 0.0;
+
+  config_->blackboard->set("goals", poses);
+
+  // create waypoint_statuses and set it on blackboard
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses(poses.goals.size());
+  for (size_t i = 0; i < waypoint_statuses.size(); ++i) {
+    waypoint_statuses[i].waypoint_pose = poses.goals[i];
+    waypoint_statuses[i].waypoint_index = i;
+  }
+  config_->blackboard->set("waypoint_statuses", waypoint_statuses);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check that it removed the point in range
+  nav_msgs::msg::Goals output_poses;
+  EXPECT_TRUE(config_->blackboard->get("goals", output_poses));
+
+  EXPECT_EQ(output_poses.goals.size(), 2u);
+  EXPECT_EQ(output_poses.goals[0], poses.goals[2]);
+  EXPECT_EQ(output_poses.goals[1], poses.goals[3]);
+
+  // check the waypoint_statuses
+  std::vector<nav2_msgs::msg::WaypointStatus> output_waypoint_statuses;
+  EXPECT_TRUE(config_->blackboard->get("waypoint_statuses", output_waypoint_statuses));
+  EXPECT_EQ(output_waypoint_statuses.size(), 4u);
+  EXPECT_EQ(output_waypoint_statuses[0].waypoint_status, nav2_msgs::msg::WaypointStatus::COMPLETED);
+  EXPECT_EQ(output_waypoint_statuses[1].waypoint_status, nav2_msgs::msg::WaypointStatus::COMPLETED);
+  EXPECT_EQ(output_waypoint_statuses[2].waypoint_status, nav2_msgs::msg::WaypointStatus::PENDING);
+  EXPECT_EQ(output_waypoint_statuses[3].waypoint_status, nav2_msgs::msg::WaypointStatus::PENDING);
+}
+
+TEST_F(
+  RemovePassedGoalsTestFixture,
+  test_tick_remove_passed_goals_find_matching_waypoint_fail)
+{
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = 0.25;
+  pose.position.y = 0.0;
+
+  transform_handler_->waitForTransform();
+  transform_handler_->updateRobotPose(pose);
+
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+          <RemovePassedGoals radius="0.5" input_goals="{goals}" output_goals="{goals}"
+                             input_waypoint_statuses="{waypoint_statuses}"
+                             output_waypoint_statuses="{waypoint_statuses}"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new goal and set it on blackboard
+  nav_msgs::msg::Goals poses;
+  poses.goals.resize(4);
+  poses.goals[0].pose.position.x = 0.0;
+  poses.goals[0].pose.position.y = 0.0;
+
+  poses.goals[1].pose.position.x = 0.5;
+  poses.goals[1].pose.position.y = 0.0;
+
+  poses.goals[2].pose.position.x = 1.0;
+  poses.goals[2].pose.position.y = 0.0;
+
+  poses.goals[3].pose.position.x = 2.0;
+  poses.goals[3].pose.position.y = 0.0;
+
+  config_->blackboard->set("goals", poses);
+
+  // create waypoint_statuses and set it on blackboard
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses(poses.goals.size());
+  for (size_t i = 0; i < waypoint_statuses.size(); ++i) {
+    waypoint_statuses[i].waypoint_pose = poses.goals[i];
+    waypoint_statuses[i].waypoint_index = i;
+  }
+  // inconsistency between waypoint_statuses and poses
+  waypoint_statuses[1].waypoint_pose.pose.position.x = 0.0;
+  config_->blackboard->set("waypoint_statuses", waypoint_statuses);
+
+  // tick until node is not running
+  tree_->rootNode()->executeTick();
+  while (tree_->rootNode()->status() == BT::NodeStatus::RUNNING) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check that it failed
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp
index 19ddaf9e9bf..9a37bffab54 100644
--- a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp
@@ -36,7 +36,7 @@ class CancelSpinServer : public TestActionServer<nav2_msgs::action::Spin>
     goal_handle)
   {
     while (!goal_handle->is_canceling()) {
-      // Spining here until goal cancels
+      // Spinning here until goal cancels
       std::this_thread::sleep_for(std::chrono::milliseconds(15));
     }
   }
@@ -135,7 +135,7 @@ TEST_F(CancelSpinActionTestFixture, test_ports)
   // Setting target yaw
   goal_msg.target_yaw = 1.57;
 
-  // Spining for server and sending a goal
+  // Spinning for server and sending a goal
   client_->wait_for_action_server();
   client_->async_send_goal(goal_msg, send_goal_options);
 
diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp
index 26ae239a180..005561f4c68 100644
--- a/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp
+++ b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp
@@ -59,7 +59,8 @@ IsStoppedTestFixture::odom_smoother_ = nullptr;
 
 TEST_F(IsStoppedTestFixture, test_behavior)
 {
-  auto odom_pub = node_->create_publisher<nav_msgs::msg::Odometry>("odom",
+  auto odom_pub = node_->create_publisher<nav_msgs::msg::Odometry>(
+    "odom",
     rclcpp::SystemDefaultsQoS());
   nav_msgs::msg::Odometry odom_msg;
 
diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp
index 8d68a5a1c79..d1deeab3bbd 100644
--- a/nav2_behavior_tree/test/test_bt_utils.cpp
+++ b/nav2_behavior_tree/test/test_bt_utils.cpp
@@ -16,7 +16,6 @@
 #include <gtest/gtest.h>
 #include <memory>
 #include <chrono>
-#include <set>
 
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
@@ -388,58 +387,161 @@ TEST(PathPortTest, test_correct_syntax)
   EXPECT_EQ(path.poses[1].pose.orientation.w, 14.0);
 }
 
-TEST(MillisecondsPortTest, test_correct_syntax)
+TEST(WaypointStatusPortTest, test_wrong_syntax)
 {
   std::string xml_txt =
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-            <MillisecondsPort test="10000" />
+            <WaypointStatusPort test="0;1;0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;msg;8.0" />
         </BehaviorTree>
       </root>)";
 
   BT::BehaviorTreeFactory factory;
-  factory.registerNodeType<TestNode<std::chrono::milliseconds>>("MillisecondsPort");
+  factory.registerNodeType<TestNode<nav2_msgs::msg::WaypointStatus>>("WaypointStatusPort");
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+
+  xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <WaypointStatusPort test="0;1;0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;" />
+        </BehaviorTree>
+      </root>)";
+
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+}
+
+TEST(WaypointStatusPortTest, test_correct_syntax)
+{
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <WaypointStatusPort test="0;1;0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;8;error" />
+        </BehaviorTree>
+      </root>)";
+
+  BT::BehaviorTreeFactory factory;
+  factory.registerNodeType<TestNode<nav2_msgs::msg::WaypointStatus>>("WaypointStatusPort");
   auto tree = factory.createTreeFromText(xml_txt);
 
-  std::chrono::milliseconds value;
-  tree.rootNode()->getInput("test", value);
-  EXPECT_EQ(value.count(), 10000);
+  nav2_msgs::msg::WaypointStatus values;
+  tree.rootNode()->getInput("test", values);
+  EXPECT_EQ(values.waypoint_status, 0);
+  EXPECT_EQ(values.waypoint_index, 1);
+  EXPECT_EQ(rclcpp::Time(values.waypoint_pose.header.stamp).nanoseconds(), 0);
+  EXPECT_EQ(values.waypoint_pose.header.frame_id, "map");
+  EXPECT_EQ(values.waypoint_pose.pose.position.x, 1.0);
+  EXPECT_EQ(values.waypoint_pose.pose.position.y, 2.0);
+  EXPECT_EQ(values.waypoint_pose.pose.position.z, 3.0);
+  EXPECT_EQ(values.waypoint_pose.pose.orientation.x, 4.0);
+  EXPECT_EQ(values.waypoint_pose.pose.orientation.y, 5.0);
+  EXPECT_EQ(values.waypoint_pose.pose.orientation.z, 6.0);
+  EXPECT_EQ(values.waypoint_pose.pose.orientation.w, 7.0);
+  EXPECT_EQ(values.error_code, 8);
+  EXPECT_EQ(values.error_msg, "error");
+}
+
+TEST(WaypointStatusVectorPortTest, test_wrong_syntax) {
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <WaypointStatusVectorPort test="0;1;0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;msg;8.0;0;1;0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;msg;8.0" />
+        </BehaviorTree>
+      </root>)";
+
+  BT::BehaviorTreeFactory factory;
+  factory.registerNodeType<TestNode<std::vector<nav2_msgs::msg::WaypointStatus>>>(
+    "WaypointStatusVectorPort");
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
 
   xml_txt =
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-            <MillisecondsPort test="123.4" />
+            <WaypointStatusVectorPort test="0;1;0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;0;1;0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;" />
         </BehaviorTree>
       </root>)";
 
-  tree = factory.createTreeFromText(xml_txt);
-  tree.rootNode()->getInput("test", value);
-  EXPECT_EQ(value.count(), 123);
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
 }
 
-TEST(ErrorCodePortTest, test_correct_syntax)
+TEST(WaypointStatusVectorPortTest, test_correct_syntax)
 {
   std::string xml_txt =
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-            <ErrorCodePort test="100;204;212"/>
+            <WaypointStatusVectorPort test="0;1;0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;8;error;9;10;0;odom;11.0;12.0;13.0;14.0;15.0;16.0;17.0;18;msg" />
         </BehaviorTree>
       </root>)";
 
   BT::BehaviorTreeFactory factory;
-  factory.registerNodeType<TestNode<std::set<int>>>("ErrorCodePort");
+  factory.registerNodeType<TestNode<std::vector<nav2_msgs::msg::WaypointStatus>>>(
+    "WaypointStatusVectorPort");
   auto tree = factory.createTreeFromText(xml_txt);
 
-  tree = factory.createTreeFromText(xml_txt);
-  std::set<int> value;
+  std::vector<nav2_msgs::msg::WaypointStatus> values;
+  tree.rootNode()->getInput("test", values);
+  EXPECT_EQ(values[0].waypoint_status, 0);
+  EXPECT_EQ(values[0].waypoint_index, 1);
+  EXPECT_EQ(rclcpp::Time(values[0].waypoint_pose.header.stamp).nanoseconds(), 0);
+  EXPECT_EQ(values[0].waypoint_pose.header.frame_id, "map");
+  EXPECT_EQ(values[0].waypoint_pose.pose.position.x, 1.0);
+  EXPECT_EQ(values[0].waypoint_pose.pose.position.y, 2.0);
+  EXPECT_EQ(values[0].waypoint_pose.pose.position.z, 3.0);
+  EXPECT_EQ(values[0].waypoint_pose.pose.orientation.x, 4.0);
+  EXPECT_EQ(values[0].waypoint_pose.pose.orientation.y, 5.0);
+  EXPECT_EQ(values[0].waypoint_pose.pose.orientation.z, 6.0);
+  EXPECT_EQ(values[0].waypoint_pose.pose.orientation.w, 7.0);
+  EXPECT_EQ(values[0].error_code, 8);
+  EXPECT_EQ(values[0].error_msg, "error");
+  EXPECT_EQ(values[1].waypoint_status, 9);
+  EXPECT_EQ(values[1].waypoint_index, 10);
+  EXPECT_EQ(rclcpp::Time(values[1].waypoint_pose.header.stamp).nanoseconds(), 0);
+  EXPECT_EQ(values[1].waypoint_pose.header.frame_id, "odom");
+  EXPECT_EQ(values[1].waypoint_pose.pose.position.x, 11.0);
+  EXPECT_EQ(values[1].waypoint_pose.pose.position.y, 12.0);
+  EXPECT_EQ(values[1].waypoint_pose.pose.position.z, 13.0);
+  EXPECT_EQ(values[1].waypoint_pose.pose.orientation.x, 14.0);
+  EXPECT_EQ(values[1].waypoint_pose.pose.orientation.y, 15.0);
+  EXPECT_EQ(values[1].waypoint_pose.pose.orientation.z, 16.0);
+  EXPECT_EQ(values[1].waypoint_pose.pose.orientation.w, 17.0);
+  EXPECT_EQ(values[1].error_code, 18);
+  EXPECT_EQ(values[1].error_msg, "msg");
+}
+
+TEST(MillisecondsPortTest, test_correct_syntax)
+{
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <MillisecondsPort test="10000" />
+        </BehaviorTree>
+      </root>)";
+
+  BT::BehaviorTreeFactory factory;
+  factory.registerNodeType<TestNode<std::chrono::milliseconds>>("MillisecondsPort");
+  auto tree = factory.createTreeFromText(xml_txt);
+
+  std::chrono::milliseconds value;
   tree.rootNode()->getInput("test", value);
+  EXPECT_EQ(value.count(), 10000);
+
+  xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <MillisecondsPort test="123.4" />
+        </BehaviorTree>
+      </root>)";
 
-  EXPECT_TRUE(value.find(100) != value.end());
-  EXPECT_TRUE(value.find(204) != value.end());
-  EXPECT_TRUE(value.find(212) != value.end());
+  tree = factory.createTreeFromText(xml_txt);
+  tree.rootNode()->getInput("test", value);
+  EXPECT_EQ(value.count(), 123);
 }
 
 TEST(deconflictPortAndParamFrameTest, test_correct_syntax)
diff --git a/nav2_behavior_tree/test/test_json_utils.cpp b/nav2_behavior_tree/test/test_json_utils.cpp
new file mode 100644
index 00000000000..7936384b2f2
--- /dev/null
+++ b/nav2_behavior_tree/test/test_json_utils.cpp
@@ -0,0 +1,1103 @@
+// Copyright (c) 2025 Alberto J. Tudela Roldán
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "behaviortree_cpp/json_export.h"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+#include "nav_msgs/msg/goals.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
+#include "nav2_msgs/msg/waypoint_status.hpp"
+
+
+class JsonTest : public testing::Test
+{
+protected:
+  JsonTest()
+  {
+    BT::JsonExporter & exporter = BT::JsonExporter::get();
+    exporter.addConverter<std::chrono::milliseconds>();
+    exporter.addConverter<builtin_interfaces::msg::Time>();
+    exporter.addConverter<std_msgs::msg::Header>();
+    exporter.addConverter<geometry_msgs::msg::Point>();
+    exporter.addConverter<geometry_msgs::msg::Quaternion>();
+    exporter.addConverter<geometry_msgs::msg::Pose>();
+    exporter.addConverter<geometry_msgs::msg::PoseStamped>();
+    exporter.addConverter<std::vector<geometry_msgs::msg::PoseStamped>>();
+    exporter.addConverter<nav_msgs::msg::Goals>();
+    exporter.addConverter<nav_msgs::msg::Path>();
+    exporter.addConverter<nav2_msgs::msg::WaypointStatus>();
+    exporter.addConverter<std::vector<nav2_msgs::msg::WaypointStatus>>();
+  }
+};
+
+TEST_F(JsonTest, test_milliseconds)
+{
+  BT::JsonExporter & exporter = BT::JsonExporter::get();
+
+  std::chrono::milliseconds milliseconds_test(1000);
+
+  nlohmann::json json;
+  exporter.toJson(BT::Any(milliseconds_test), json["ms"]);
+
+  std::cout << json.dump(2) << std::endl;
+
+  ASSERT_EQ(json["ms"]["__type"], "std::chrono::milliseconds");
+  ASSERT_EQ(json["ms"]["ms"], 1000);
+
+  // Check the two-ways transform, i.e. "from_json"
+  auto milliseconds_test2 =
+    exporter.fromJson(json["ms"])->first.cast<std::chrono::milliseconds>();
+
+  ASSERT_EQ(milliseconds_test.count(), milliseconds_test2.count());
+
+  // Convert from string
+  std::chrono::milliseconds milliseconds_test3;
+  auto const test_json = R"(json:{"__type": "std::chrono::milliseconds", "ms": 1000})";
+  ASSERT_NO_THROW(milliseconds_test3 = BT::convertFromString<std::chrono::milliseconds>(test_json));
+
+  ASSERT_EQ(milliseconds_test.count(), milliseconds_test3.count());
+}
+
+TEST_F(JsonTest, test_time)
+{
+  BT::JsonExporter & exporter = BT::JsonExporter::get();
+
+  builtin_interfaces::msg::Time time_test;
+  time_test.sec = 1;
+  time_test.nanosec = 2;
+
+  nlohmann::json json;
+  exporter.toJson(BT::Any(time_test), json["time"]);
+
+  std::cout << json.dump(2) << std::endl;
+
+  ASSERT_EQ(json["time"]["__type"], "builtin_interfaces::msg::Time");
+  ASSERT_EQ(json["time"]["sec"], 1);
+  ASSERT_EQ(json["time"]["nanosec"], 2);
+
+  // Check the two-ways transform, i.e. "from_json"
+  auto time_test2 = exporter.fromJson(json["time"])->first.cast<builtin_interfaces::msg::Time>();
+
+  ASSERT_EQ(time_test.sec, time_test2.sec);
+  ASSERT_EQ(time_test.nanosec, time_test2.nanosec);
+
+  // Convert from string
+  builtin_interfaces::msg::Time time_test3;
+  auto const test_json =
+    R"(json:{"__type": "builtin_interfaces::msg::Time", "sec": 1, "nanosec": 2})";
+  ASSERT_NO_THROW(time_test3 = BT::convertFromString<builtin_interfaces::msg::Time>(test_json));
+
+  ASSERT_EQ(time_test.sec, time_test3.sec);
+  ASSERT_EQ(time_test.nanosec, time_test3.nanosec);
+}
+
+TEST_F(JsonTest, test_header)
+{
+  BT::JsonExporter & exporter = BT::JsonExporter::get();
+
+  std_msgs::msg::Header header_test;
+  header_test.stamp.sec = 1;
+  header_test.stamp.nanosec = 2;
+  header_test.frame_id = "map";
+
+  nlohmann::json json;
+  exporter.toJson(BT::Any(header_test), json["header"]);
+
+  std::cout << json.dump(2) << std::endl;
+
+  ASSERT_EQ(json["header"]["__type"], "std_msgs::msg::Header");
+  ASSERT_EQ(json["header"]["stamp"]["__type"], "builtin_interfaces::msg::Time");
+  ASSERT_EQ(json["header"]["stamp"]["sec"], 1);
+  ASSERT_EQ(json["header"]["stamp"]["nanosec"], 2);
+  ASSERT_EQ(json["header"]["frame_id"], "map");
+
+  // Check the two-ways transform, i.e. "from_json"
+  auto header_test2 = exporter.fromJson(json["header"])->first.cast<std_msgs::msg::Header>();
+
+  ASSERT_EQ(header_test.stamp.sec, header_test2.stamp.sec);
+  ASSERT_EQ(header_test.stamp.nanosec, header_test2.stamp.nanosec);
+  ASSERT_EQ(header_test.frame_id, header_test2.frame_id);
+
+  // Convert from string
+  std_msgs::msg::Header header_test3;
+  auto const test_json =
+    R"(json:
+      {
+        "__type": "std_msgs::msg::Header",
+        "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 1, "nanosec": 2},
+        "frame_id": "map"
+      }
+    )";
+  ASSERT_NO_THROW(header_test3 = BT::convertFromString<std_msgs::msg::Header>(test_json));
+  ASSERT_EQ(header_test.stamp.sec, header_test3.stamp.sec);
+  ASSERT_EQ(header_test.stamp.nanosec, header_test3.stamp.nanosec);
+  ASSERT_EQ(header_test.frame_id, header_test3.frame_id);
+}
+
+TEST_F(JsonTest, test_point)
+{
+  BT::JsonExporter & exporter = BT::JsonExporter::get();
+
+  geometry_msgs::msg::Point point_test;
+  point_test.x = 1.0;
+  point_test.y = 2.0;
+  point_test.z = 3.0;
+
+  nlohmann::json json;
+  exporter.toJson(BT::Any(point_test), json["point"]);
+
+  std::cout << json.dump(2) << std::endl;
+
+  ASSERT_EQ(json["point"]["__type"], "geometry_msgs::msg::Point");
+  ASSERT_EQ(json["point"]["x"], 1.0);
+  ASSERT_EQ(json["point"]["y"], 2.0);
+  ASSERT_EQ(json["point"]["z"], 3.0);
+
+  // Check the two-ways transform, i.e. "from_json"
+  auto point_test2 = exporter.fromJson(json["point"])->first.cast<geometry_msgs::msg::Point>();
+
+  ASSERT_EQ(point_test.x, point_test2.x);
+  ASSERT_EQ(point_test.y, point_test2.y);
+  ASSERT_EQ(point_test.z, point_test2.z);
+
+  // Convert from string
+  geometry_msgs::msg::Point point_test3;
+  auto const test_json =
+    R"(json:
+      {
+        "__type": "geometry_msgs::msg::Point",
+        "x": 1.0, "y": 2.0, "z": 3.0
+      }
+    )";
+  ASSERT_NO_THROW(point_test3 = BT::convertFromString<geometry_msgs::msg::Point>(test_json));
+  ASSERT_EQ(point_test.x, point_test3.x);
+  ASSERT_EQ(point_test.y, point_test3.y);
+  ASSERT_EQ(point_test.z, point_test3.z);
+}
+
+TEST_F(JsonTest, test_quaternion)
+{
+  BT::JsonExporter & exporter = BT::JsonExporter::get();
+
+  geometry_msgs::msg::Quaternion quaternion_test;
+  quaternion_test.x = 1.0;
+  quaternion_test.y = 2.0;
+  quaternion_test.z = 3.0;
+  quaternion_test.w = 4.0;
+
+  nlohmann::json json;
+  exporter.toJson(BT::Any(quaternion_test), json["quaternion"]);
+
+  std::cout << json.dump(2) << std::endl;
+
+  ASSERT_EQ(json["quaternion"]["__type"], "geometry_msgs::msg::Quaternion");
+  ASSERT_EQ(json["quaternion"]["x"], 1.0);
+  ASSERT_EQ(json["quaternion"]["y"], 2.0);
+  ASSERT_EQ(json["quaternion"]["z"], 3.0);
+  ASSERT_EQ(json["quaternion"]["w"], 4.0);
+
+  // Check the two-ways transform, i.e. "from_json"
+  auto quaternion_test2 =
+    exporter.fromJson(json["quaternion"])->first.cast<geometry_msgs::msg::Quaternion>();
+
+  ASSERT_EQ(quaternion_test.x, quaternion_test2.x);
+  ASSERT_EQ(quaternion_test.y, quaternion_test2.y);
+  ASSERT_EQ(quaternion_test.z, quaternion_test2.z);
+  ASSERT_EQ(quaternion_test.w, quaternion_test2.w);
+
+  // Convert from string
+  geometry_msgs::msg::Quaternion quaternion_test3;
+  auto const test_json =
+    R"(json:
+      {
+        "__type": "geometry_msgs::msg::Quaternion",
+        "x": 1.0, "y": 2.0, "z": 3.0, "w": 4.0
+      }
+    )";
+  ASSERT_NO_THROW(
+    quaternion_test3 =
+    BT::convertFromString<geometry_msgs::msg::Quaternion>(test_json));
+  ASSERT_EQ(quaternion_test.x, quaternion_test3.x);
+  ASSERT_EQ(quaternion_test.y, quaternion_test3.y);
+  ASSERT_EQ(quaternion_test.z, quaternion_test3.z);
+  ASSERT_EQ(quaternion_test.w, quaternion_test3.w);
+}
+
+TEST_F(JsonTest, test_pose_stamped)
+{
+  BT::JsonExporter & exporter = BT::JsonExporter::get();
+
+  geometry_msgs::msg::PoseStamped pose_stamped_test;
+  pose_stamped_test.header.stamp.sec = 1;
+  pose_stamped_test.header.stamp.nanosec = 2;
+  pose_stamped_test.header.frame_id = "map";
+  pose_stamped_test.pose.position.x = 3.0;
+  pose_stamped_test.pose.position.y = 4.0;
+  pose_stamped_test.pose.position.z = 5.0;
+  pose_stamped_test.pose.orientation.x = 6.0;
+  pose_stamped_test.pose.orientation.y = 7.0;
+  pose_stamped_test.pose.orientation.z = 8.0;
+  pose_stamped_test.pose.orientation.w = 9.0;
+
+  nlohmann::json json;
+  exporter.toJson(BT::Any(pose_stamped_test), json["pose"]);
+
+  std::cout << json.dump(2) << std::endl;
+
+  ASSERT_EQ(json["pose"]["__type"], "geometry_msgs::msg::PoseStamped");
+  ASSERT_EQ(json["pose"]["header"]["__type"], "std_msgs::msg::Header");
+  ASSERT_EQ(json["pose"]["header"]["stamp"]["__type"], "builtin_interfaces::msg::Time");
+  ASSERT_EQ(json["pose"]["header"]["stamp"]["sec"], 1);
+  ASSERT_EQ(json["pose"]["header"]["stamp"]["nanosec"], 2);
+  ASSERT_EQ(json["pose"]["header"]["frame_id"], "map");
+  ASSERT_EQ(json["pose"]["pose"]["__type"], "geometry_msgs::msg::Pose");
+  ASSERT_EQ(json["pose"]["pose"]["position"]["__type"], "geometry_msgs::msg::Point");
+  ASSERT_EQ(json["pose"]["pose"]["position"]["x"], 3.0);
+  ASSERT_EQ(json["pose"]["pose"]["position"]["y"], 4.0);
+  ASSERT_EQ(json["pose"]["pose"]["position"]["z"], 5.0);
+  ASSERT_EQ(json["pose"]["pose"]["orientation"]["__type"], "geometry_msgs::msg::Quaternion");
+  ASSERT_EQ(json["pose"]["pose"]["orientation"]["x"], 6.0);
+  ASSERT_EQ(json["pose"]["pose"]["orientation"]["y"], 7.0);
+  ASSERT_EQ(json["pose"]["pose"]["orientation"]["z"], 8.0);
+  ASSERT_EQ(json["pose"]["pose"]["orientation"]["w"], 9.0);
+
+  // Check the two-ways transform, i.e. "from_json"
+  auto pose_stamped_test2 =
+    exporter.fromJson(json["pose"])->first.cast<geometry_msgs::msg::PoseStamped>();
+
+  ASSERT_EQ(pose_stamped_test.header.stamp.sec, pose_stamped_test2.header.stamp.sec);
+  ASSERT_EQ(pose_stamped_test.header.stamp.nanosec, pose_stamped_test2.header.stamp.nanosec);
+  ASSERT_EQ(pose_stamped_test.header.frame_id, pose_stamped_test2.header.frame_id);
+  ASSERT_EQ(pose_stamped_test.pose.position.x, pose_stamped_test2.pose.position.x);
+  ASSERT_EQ(pose_stamped_test.pose.position.y, pose_stamped_test2.pose.position.y);
+  ASSERT_EQ(pose_stamped_test.pose.position.z, pose_stamped_test2.pose.position.z);
+  ASSERT_EQ(pose_stamped_test.pose.orientation.x, pose_stamped_test2.pose.orientation.x);
+  ASSERT_EQ(pose_stamped_test.pose.orientation.y, pose_stamped_test2.pose.orientation.y);
+  ASSERT_EQ(pose_stamped_test.pose.orientation.z, pose_stamped_test2.pose.orientation.z);
+  ASSERT_EQ(pose_stamped_test.pose.orientation.w, pose_stamped_test2.pose.orientation.w);
+
+  // Convert from string
+  geometry_msgs::msg::PoseStamped pose_stamped_test3;
+  auto const test_json =
+    R"(json:
+      {
+        "__type": "geometry_msgs::msg::PoseStamped",
+        "header": {
+          "__type": "std_msgs::msg::Header",
+          "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 1, "nanosec": 2},
+          "frame_id": "map"
+        },
+        "pose": {
+          "__type": "geometry_msgs::msg::Pose",
+          "position": {
+            "__type": "geometry_msgs::msg::Point",
+            "x": 3.0, "y": 4.0, "z": 5.0
+          },
+          "orientation": {
+            "__type": "geometry_msgs::msg::Quaternion",
+            "x": 6.0, "y": 7.0, "z": 8.0, "w": 9.0
+          }
+        }
+      }
+    )";
+  ASSERT_NO_THROW(
+    pose_stamped_test3 =
+    BT::convertFromString<geometry_msgs::msg::PoseStamped>(test_json));
+  ASSERT_EQ(pose_stamped_test.header, pose_stamped_test3.header);
+  ASSERT_EQ(pose_stamped_test.pose.position, pose_stamped_test3.pose.position);
+  ASSERT_EQ(pose_stamped_test.pose.orientation, pose_stamped_test3.pose.orientation);
+}
+
+TEST_F(JsonTest, test_pose_stamped_vector)
+{
+  BT::JsonExporter & exporter = BT::JsonExporter::get();
+
+  std::vector<geometry_msgs::msg::PoseStamped> pose_stamped_vector_test;
+  pose_stamped_vector_test.resize(2);
+  pose_stamped_vector_test[0].header.stamp.sec = 1;
+  pose_stamped_vector_test[0].header.stamp.nanosec = 2;
+  pose_stamped_vector_test[0].header.frame_id = "map";
+  pose_stamped_vector_test[0].pose.position.x = 3.0;
+  pose_stamped_vector_test[0].pose.position.y = 4.0;
+  pose_stamped_vector_test[0].pose.position.z = 5.0;
+  pose_stamped_vector_test[0].pose.orientation.x = 6.0;
+  pose_stamped_vector_test[0].pose.orientation.y = 7.0;
+  pose_stamped_vector_test[0].pose.orientation.z = 8.0;
+  pose_stamped_vector_test[0].pose.orientation.w = 9.0;
+
+  pose_stamped_vector_test[1].header.stamp.sec = 10;
+  pose_stamped_vector_test[1].header.stamp.nanosec = 11;
+  pose_stamped_vector_test[1].header.frame_id = "odom";
+  pose_stamped_vector_test[1].pose.position.x = 12.0;
+  pose_stamped_vector_test[1].pose.position.y = 13.0;
+  pose_stamped_vector_test[1].pose.position.z = 14.0;
+  pose_stamped_vector_test[1].pose.orientation.x = 15.0;
+  pose_stamped_vector_test[1].pose.orientation.y = 16.0;
+  pose_stamped_vector_test[1].pose.orientation.z = 17.0;
+  pose_stamped_vector_test[1].pose.orientation.w = 18.0;
+
+  nlohmann::json json;
+  exporter.toJson(BT::Any(pose_stamped_vector_test), json["poses"]);
+
+  std::cout << json.dump(2) << std::endl;
+
+  ASSERT_EQ(json["poses"][0]["__type"], "geometry_msgs::msg::PoseStamped");
+  ASSERT_EQ(json["poses"][0]["header"]["__type"], "std_msgs::msg::Header");
+  ASSERT_EQ(json["poses"][0]["header"]["stamp"]["__type"], "builtin_interfaces::msg::Time");
+  ASSERT_EQ(json["poses"][0]["header"]["stamp"]["sec"], 1);
+  ASSERT_EQ(json["poses"][0]["header"]["stamp"]["nanosec"], 2);
+  ASSERT_EQ(json["poses"][0]["header"]["frame_id"], "map");
+  ASSERT_EQ(json["poses"][0]["pose"]["__type"], "geometry_msgs::msg::Pose");
+  ASSERT_EQ(json["poses"][0]["pose"]["position"]["__type"], "geometry_msgs::msg::Point");
+  ASSERT_EQ(json["poses"][0]["pose"]["position"]["x"], 3.0);
+  ASSERT_EQ(json["poses"][0]["pose"]["position"]["y"], 4.0);
+  ASSERT_EQ(json["poses"][0]["pose"]["position"]["z"], 5.0);
+  ASSERT_EQ(
+    json["poses"][0]["pose"]["orientation"]["__type"],
+    "geometry_msgs::msg::Quaternion");
+  ASSERT_EQ(json["poses"][0]["pose"]["orientation"]["x"], 6.0);
+  ASSERT_EQ(json["poses"][0]["pose"]["orientation"]["y"], 7.0);
+  ASSERT_EQ(json["poses"][0]["pose"]["orientation"]["z"], 8.0);
+  ASSERT_EQ(json["poses"][0]["pose"]["orientation"]["w"], 9.0);
+  ASSERT_EQ(json["poses"][1]["__type"], "geometry_msgs::msg::PoseStamped");
+  ASSERT_EQ(json["poses"][1]["header"]["__type"], "std_msgs::msg::Header");
+  ASSERT_EQ(
+    json["poses"][1]["header"]["stamp"]["__type"],
+    "builtin_interfaces::msg::Time");
+  ASSERT_EQ(json["poses"][1]["header"]["stamp"]["sec"], 10);
+  ASSERT_EQ(json["poses"][1]["header"]["stamp"]["nanosec"], 11);
+  ASSERT_EQ(json["poses"][1]["header"]["frame_id"], "odom");
+  ASSERT_EQ(json["poses"][1]["pose"]["__type"], "geometry_msgs::msg::Pose");
+  ASSERT_EQ(
+    json["poses"][1]["pose"]["position"]["__type"],
+    "geometry_msgs::msg::Point");
+  ASSERT_EQ(json["poses"][1]["pose"]["position"]["x"], 12.0);
+  ASSERT_EQ(json["poses"][1]["pose"]["position"]["y"], 13.0);
+  ASSERT_EQ(json["poses"][1]["pose"]["position"]["z"], 14.0);
+  ASSERT_EQ(
+    json["poses"][1]["pose"]["orientation"]["__type"],
+    "geometry_msgs::msg::Quaternion");
+  ASSERT_EQ(json["poses"][1]["pose"]["orientation"]["x"], 15.0);
+  ASSERT_EQ(json["poses"][1]["pose"]["orientation"]["y"], 16.0);
+  ASSERT_EQ(json["poses"][1]["pose"]["orientation"]["z"], 17.0);
+  ASSERT_EQ(json["poses"][1]["pose"]["orientation"]["w"], 18.0);
+
+  // Check the two-ways transform, i.e. "from_json"
+  // auto pose_stamped_vector_test2 =
+  //   exporter.fromJson(json["poses"])->first.cast<std::vector<geometry_msgs::msg::PoseStamped>>();
+  // ASSERT_EQ(pose_stamped_vector_test[0].header.stamp.sec,
+  //   pose_stamped_vector_test2[0].header.stamp.sec);
+  // ASSERT_EQ(pose_stamped_vector_test[0].header.stamp.nanosec,
+  //   pose_stamped_vector_test2[0].header.stamp.nanosec);
+  // ASSERT_EQ(pose_stamped_vector_test[0].header.frame_id,
+  //   pose_stamped_vector_test2[0].header.frame_id);
+  // ASSERT_EQ(pose_stamped_vector_test[0].pose.position.x,
+  //   pose_stamped_vector_test2[0].pose.position.x);
+  // ASSERT_EQ(pose_stamped_vector_test[0].pose.position.y,
+  //   pose_stamped_vector_test2[0].pose.position.y);
+  // ASSERT_EQ(pose_stamped_vector_test[0].pose.position.z,
+  //   pose_stamped_vector_test2[0].pose.position.z);
+  // ASSERT_EQ(pose_stamped_vector_test[0].pose.orientation.x,
+  //   pose_stamped_vector_test2[0].pose.orientation.x);
+  // ASSERT_EQ(pose_stamped_vector_test[0].pose.orientation.y,
+  //   pose_stamped_vector_test2[0].pose.orientation.y);
+  // ASSERT_EQ(pose_stamped_vector_test[0].pose.orientation.z,
+  //   pose_stamped_vector_test2[0].pose.orientation.z);
+  // ASSERT_EQ(pose_stamped_vector_test[0].pose.orientation.w,
+  //   pose_stamped_vector_test2[0].pose.orientation.w);
+  // ASSERT_EQ(pose_stamped_vector_test[1].header.stamp.sec,
+  //   pose_stamped_vector_test2[1].header.stamp.sec);
+  // ASSERT_EQ(pose_stamped_vector_test[1].header.stamp.nanosec,
+  //   pose_stamped_vector_test2[1].header.stamp.nanosec);
+  // ASSERT_EQ(pose_stamped_vector_test[1].header.frame_id,
+  //   pose_stamped_vector_test2[1].header.frame_id);
+  // ASSERT_EQ(pose_stamped_vector_test[1].pose.position.x,
+  //   pose_stamped_vector_test2[1].pose.position.x);
+  // ASSERT_EQ(pose_stamped_vector_test[1].pose.position.y,
+  //   pose_stamped_vector_test2[1].pose.position.y);
+  // ASSERT_EQ(pose_stamped_vector_test[1].pose.position.z,
+  //   pose_stamped_vector_test2[1].pose.position.z);
+  // ASSERT_EQ(pose_stamped_vector_test[1].pose.orientation.x,
+  //   pose_stamped_vector_test2[1].pose.orientation.x);
+  // ASSERT_EQ(pose_stamped_vector_test[1].pose.orientation.y,
+  //   pose_stamped_vector_test2[1].pose.orientation.y);
+  // ASSERT_EQ(pose_stamped_vector_test[1].pose.orientation.z,
+  //   pose_stamped_vector_test2[1].pose.orientation.z);
+  // ASSERT_EQ(pose_stamped_vector_test[1].pose.orientation.w,
+  //   pose_stamped_vector_test2[1].pose.orientation.w);
+
+  // // Convert from string
+  // std::vector<geometry_msgs::msg::PoseStamped> pose_stamped_vector_test3;
+  // auto const test_json =
+  //   R"(json:
+  //     {
+  //       "poses": [
+  //         {
+  //           "__type": "geometry_msgs::msg::PoseStamped",
+  //           "header": {
+  //             "__type": "std_msgs::msg::Header",
+  //             "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 1, "nanosec": 2},
+  //             "frame_id": "map"
+  //           },
+  //           "pose": {
+  //             "__type": "geometry_msgs::msg::Pose",
+  //             "position": {
+  //               "__type": "geometry_msgs::msg::Point",
+  //               "x": 3.0, "y": 4.0, "z": 5.0
+  //             },
+  //             "orientation": {
+  //               "__type": "geometry_msgs::msg::Quaternion",
+  //               "x": 6.0, "y": 7.0, "z": 8.0, "w": 9.0
+  //             }
+  //           }
+  //         },
+  //         {
+  //           "__type": "geometry_msgs::msg::PoseStamped",
+  //           "header": {
+  //             "__type": "std_msgs::msg::Header",
+  //             "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 10, "nanosec": 11},
+  //             "frame_id": "odom"
+  //           },
+  //           "pose": {
+  //             "__type": "geometry_msgs::msg::Pose",
+  //             "position": {
+  //               "__type": "geometry_msgs::msg::Point",
+  //               "x": 12.0, "y": 13.0, "z": 14.0
+  //             },
+  //             "orientation": {
+  //               "__type": "geometry_msgs::msg::Quaternion",
+  //               "x": 15.0, "y": 16.0, "z": 17.0, "w": 18.0
+  //             }
+  //           }
+  //         }
+  //       ]
+  //     }
+  //   )";
+  // ASSERT_NO_THROW(pose_stamped_vector_test3 =
+  //   BT::convertFromString<std::vector<geometry_msgs::msg::PoseStamped>>(test_json));
+  // ASSERT_EQ(pose_stamped_vector_test[0].header, pose_stamped_vector_test3[0].header);
+  // ASSERT_EQ(pose_stamped_vector_test[0].pose.position,
+  //   pose_stamped_vector_test3[0].pose.position);
+  // ASSERT_EQ(pose_stamped_vector_test[0].pose.orientation,
+  //   pose_stamped_vector_test3[0].pose.orientation);
+  // ASSERT_EQ(pose_stamped_vector_test[1].header, pose_stamped_vector_test3[1].header);
+  // ASSERT_EQ(pose_stamped_vector_test[1].pose.position,
+  //   pose_stamped_vector_test3[1].pose.position);
+  // ASSERT_EQ(pose_stamped_vector_test[1].pose.orientation,
+  //   pose_stamped_vector_test3[1].pose.orientation);
+}
+
+TEST_F(JsonTest, test_goals)
+{
+  BT::JsonExporter & exporter = BT::JsonExporter::get();
+
+  nav_msgs::msg::Goals goals_test;
+  goals_test.header.stamp.sec = 1;
+  goals_test.header.stamp.nanosec = 2;
+  goals_test.header.frame_id = "map";
+  goals_test.goals.resize(2);
+  goals_test.goals[0].header.stamp.sec = 3;
+  goals_test.goals[0].header.stamp.nanosec = 4;
+  goals_test.goals[0].header.frame_id = "map";
+  goals_test.goals[0].pose.position.x = 5.0;
+  goals_test.goals[0].pose.position.y = 6.0;
+  goals_test.goals[0].pose.position.z = 7.0;
+  goals_test.goals[0].pose.orientation.x = 8.0;
+  goals_test.goals[0].pose.orientation.y = 9.0;
+  goals_test.goals[0].pose.orientation.z = 10.0;
+  goals_test.goals[0].pose.orientation.w = 11.0;
+
+  goals_test.goals[1].header.stamp.sec = 12;
+  goals_test.goals[1].header.stamp.nanosec = 13;
+  goals_test.goals[1].header.frame_id = "odom";
+  goals_test.goals[1].pose.position.x = 14.0;
+  goals_test.goals[1].pose.position.y = 15.0;
+  goals_test.goals[1].pose.position.z = 16.0;
+  goals_test.goals[1].pose.orientation.x = 17.0;
+  goals_test.goals[1].pose.orientation.y = 18.0;
+  goals_test.goals[1].pose.orientation.z = 19.0;
+  goals_test.goals[1].pose.orientation.w = 20.0;
+
+  nlohmann::json json;
+  exporter.toJson(BT::Any(goals_test), json["goals"]);
+
+  std::cout << json.dump(2) << std::endl;
+
+  ASSERT_EQ(json["goals"]["__type"], "nav_msgs::msg::Goals");
+  ASSERT_EQ(json["goals"]["header"]["__type"], "std_msgs::msg::Header");
+  ASSERT_EQ(json["goals"]["header"]["stamp"]["__type"], "builtin_interfaces::msg::Time");
+  ASSERT_EQ(json["goals"]["header"]["stamp"]["sec"], 1);
+  ASSERT_EQ(json["goals"]["header"]["stamp"]["nanosec"], 2);
+  ASSERT_EQ(json["goals"]["header"]["frame_id"], "map");
+  ASSERT_EQ(json["goals"]["goals"][0]["__type"], "geometry_msgs::msg::PoseStamped");
+  ASSERT_EQ(json["goals"]["goals"][0]["header"]["__type"], "std_msgs::msg::Header");
+  ASSERT_EQ(
+    json["goals"]["goals"][0]["header"]["stamp"]["__type"],
+    "builtin_interfaces::msg::Time");
+  ASSERT_EQ(json["goals"]["goals"][0]["header"]["stamp"]["sec"], 3);
+  ASSERT_EQ(json["goals"]["goals"][0]["header"]["stamp"]["nanosec"], 4);
+  ASSERT_EQ(json["goals"]["goals"][0]["header"]["frame_id"], "map");
+  ASSERT_EQ(json["goals"]["goals"][0]["pose"]["__type"], "geometry_msgs::msg::Pose");
+  ASSERT_EQ(json["goals"]["goals"][0]["pose"]["position"]["__type"], "geometry_msgs::msg::Point");
+  ASSERT_EQ(json["goals"]["goals"][0]["pose"]["position"]["x"], 5.0);
+  ASSERT_EQ(json["goals"]["goals"][0]["pose"]["position"]["y"], 6.0);
+  ASSERT_EQ(json["goals"]["goals"][0]["pose"]["position"]["z"], 7.0);
+  ASSERT_EQ(
+    json["goals"]["goals"][0]["pose"]["orientation"]["__type"],
+    "geometry_msgs::msg::Quaternion");
+  ASSERT_EQ(json["goals"]["goals"][0]["pose"]["orientation"]["x"], 8.0);
+  ASSERT_EQ(json["goals"]["goals"][0]["pose"]["orientation"]["y"], 9.0);
+  ASSERT_EQ(json["goals"]["goals"][0]["pose"]["orientation"]["z"], 10.0);
+  ASSERT_EQ(json["goals"]["goals"][0]["pose"]["orientation"]["w"], 11.0);
+  ASSERT_EQ(json["goals"]["goals"][1]["__type"], "geometry_msgs::msg::PoseStamped");
+  ASSERT_EQ(json["goals"]["goals"][1]["header"]["__type"], "std_msgs::msg::Header");
+  ASSERT_EQ(
+    json["goals"]["goals"][1]["header"]["stamp"]["__type"],
+    "builtin_interfaces::msg::Time");
+  ASSERT_EQ(json["goals"]["goals"][1]["header"]["stamp"]["sec"], 12);
+  ASSERT_EQ(json["goals"]["goals"][1]["header"]["stamp"]["nanosec"], 13);
+  ASSERT_EQ(json["goals"]["goals"][1]["header"]["frame_id"], "odom");
+  ASSERT_EQ(json["goals"]["goals"][1]["pose"]["__type"], "geometry_msgs::msg::Pose");
+  ASSERT_EQ(json["goals"]["goals"][1]["pose"]["position"]["__type"], "geometry_msgs::msg::Point");
+  ASSERT_EQ(json["goals"]["goals"][1]["pose"]["position"]["x"], 14.0);
+  ASSERT_EQ(json["goals"]["goals"][1]["pose"]["position"]["y"], 15.0);
+  ASSERT_EQ(json["goals"]["goals"][1]["pose"]["position"]["z"], 16.0);
+  ASSERT_EQ(
+    json["goals"]["goals"][1]["pose"]["orientation"]["__type"],
+    "geometry_msgs::msg::Quaternion");
+  ASSERT_EQ(json["goals"]["goals"][1]["pose"]["orientation"]["x"], 17.0);
+  ASSERT_EQ(json["goals"]["goals"][1]["pose"]["orientation"]["y"], 18.0);
+  ASSERT_EQ(json["goals"]["goals"][1]["pose"]["orientation"]["z"], 19.0);
+  ASSERT_EQ(json["goals"]["goals"][1]["pose"]["orientation"]["w"], 20.0);
+
+  // Check the two-ways transform, i.e. "from_json"
+  auto goals_test2 = exporter.fromJson(json["goals"])->first.cast<nav_msgs::msg::Goals>();
+
+  ASSERT_EQ(goals_test.goals[0].header.stamp.sec, goals_test2.goals[0].header.stamp.sec);
+  ASSERT_EQ(goals_test.goals[0].header.stamp.nanosec, goals_test2.goals[0].header.stamp.nanosec);
+  ASSERT_EQ(goals_test.goals[0].header.frame_id, goals_test2.goals[0].header.frame_id);
+  ASSERT_EQ(goals_test.goals[0].pose.position.x, goals_test2.goals[0].pose.position.x);
+  ASSERT_EQ(goals_test.goals[0].pose.position.y, goals_test2.goals[0].pose.position.y);
+  ASSERT_EQ(goals_test.goals[0].pose.position.z, goals_test2.goals[0].pose.position.z);
+  ASSERT_EQ(goals_test.goals[0].pose.orientation.x, goals_test2.goals[0].pose.orientation.x);
+  ASSERT_EQ(goals_test.goals[0].pose.orientation.y, goals_test2.goals[0].pose.orientation.y);
+  ASSERT_EQ(goals_test.goals[0].pose.orientation.z, goals_test2.goals[0].pose.orientation.z);
+  ASSERT_EQ(goals_test.goals[0].pose.orientation.w, goals_test2.goals[0].pose.orientation.w);
+  ASSERT_EQ(goals_test.goals[1].header.stamp.sec, goals_test2.goals[1].header.stamp.sec);
+  ASSERT_EQ(goals_test.goals[1].header.stamp.nanosec, goals_test2.goals[1].header.stamp.nanosec);
+  ASSERT_EQ(goals_test.goals[1].header.frame_id, goals_test2.goals[1].header.frame_id);
+  ASSERT_EQ(goals_test.goals[1].pose.position.x, goals_test2.goals[1].pose.position.x);
+  ASSERT_EQ(goals_test.goals[1].pose.position.y, goals_test2.goals[1].pose.position.y);
+  ASSERT_EQ(goals_test.goals[1].pose.position.z, goals_test2.goals[1].pose.position.z);
+  ASSERT_EQ(goals_test.goals[1].pose.orientation.x, goals_test2.goals[1].pose.orientation.x);
+  ASSERT_EQ(goals_test.goals[1].pose.orientation.y, goals_test2.goals[1].pose.orientation.y);
+  ASSERT_EQ(goals_test.goals[1].pose.orientation.z, goals_test2.goals[1].pose.orientation.z);
+  ASSERT_EQ(goals_test.goals[1].pose.orientation.w, goals_test2.goals[1].pose.orientation.w);
+
+  // Convert from string
+  nav_msgs::msg::Goals goals_test3;
+  auto const test_json =
+    R"(json:
+      {
+        "__type": "nav_msgs::msg::Goals",
+        "header": {
+          "__type": "std_msgs::msg::Header",
+          "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 1, "nanosec": 2},
+          "frame_id": "map"
+        },
+        "goals": [
+          {
+            "__type": "geometry_msgs::msg::PoseStamped",
+            "header": {
+              "__type": "std_msgs::msg::Header",
+              "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 3, "nanosec": 4},
+              "frame_id": "map"
+            },
+            "pose": {
+              "__type": "geometry_msgs::msg::Pose",
+              "position": {
+                "__type": "geometry_msgs::msg::Point",
+                "x": 5.0, "y": 6.0, "z": 7.0
+              },
+              "orientation": {
+                "__type": "geometry_msgs::msg::Quaternion",
+                "x": 8.0, "y": 9.0, "z": 10.0, "w": 11.0
+              }
+            }
+          },
+          {
+            "__type": "geometry_msgs::msg::PoseStamped",
+            "header": {
+              "__type": "std_msgs::msg::Header",
+              "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 12, "nanosec": 13},
+              "frame_id": "odom"
+            },
+            "pose": {
+              "__type": "geometry_msgs::msg::Pose",
+              "position": {
+                "__type": "geometry_msgs::msg::Point",
+                "x": 14.0, "y": 15.0, "z": 16.0
+              },
+              "orientation": {
+                "__type": "geometry_msgs::msg::Quaternion",
+                "x": 17.0, "y": 18.0, "z": 19.0, "w": 20.0
+              }
+            }
+          }
+        ]
+      }
+    )";
+  ASSERT_NO_THROW(
+    goals_test3 =
+    BT::convertFromString<nav_msgs::msg::Goals>(test_json));
+  ASSERT_EQ(goals_test.goals[0].header, goals_test3.goals[0].header);
+  ASSERT_EQ(goals_test.goals[0].pose.position, goals_test3.goals[0].pose.position);
+  ASSERT_EQ(goals_test.goals[0].pose.orientation, goals_test3.goals[0].pose.orientation);
+  ASSERT_EQ(goals_test.goals[1].header, goals_test3.goals[1].header);
+  ASSERT_EQ(goals_test.goals[1].pose.position, goals_test3.goals[1].pose.position);
+  ASSERT_EQ(goals_test.goals[1].pose.orientation, goals_test3.goals[1].pose.orientation);
+}
+
+TEST_F(JsonTest, test_path)
+{
+  BT::JsonExporter & exporter = BT::JsonExporter::get();
+
+  nav_msgs::msg::Path path_test;
+  path_test.header.stamp.sec = 1;
+  path_test.header.stamp.nanosec = 2;
+  path_test.header.frame_id = "map";
+  path_test.poses.resize(2);
+  path_test.poses[0].header.stamp.sec = 3;
+  path_test.poses[0].header.stamp.nanosec = 4;
+  path_test.poses[0].header.frame_id = "map";
+  path_test.poses[0].pose.position.x = 5.0;
+  path_test.poses[0].pose.position.y = 6.0;
+  path_test.poses[0].pose.position.z = 7.0;
+  path_test.poses[0].pose.orientation.x = 8.0;
+  path_test.poses[0].pose.orientation.y = 9.0;
+  path_test.poses[0].pose.orientation.z = 10.0;
+  path_test.poses[0].pose.orientation.w = 11.0;
+
+  path_test.poses[1].header.stamp.sec = 12;
+  path_test.poses[1].header.stamp.nanosec = 13;
+  path_test.poses[1].header.frame_id = "odom";
+  path_test.poses[1].pose.position.x = 14.0;
+  path_test.poses[1].pose.position.y = 15.0;
+  path_test.poses[1].pose.position.z = 16.0;
+  path_test.poses[1].pose.orientation.x = 17.0;
+  path_test.poses[1].pose.orientation.y = 18.0;
+  path_test.poses[1].pose.orientation.z = 19.0;
+  path_test.poses[1].pose.orientation.w = 20.0;
+
+  nlohmann::json json;
+  exporter.toJson(BT::Any(path_test), json["path"]);
+
+  std::cout << json.dump(2) << std::endl;
+
+  ASSERT_EQ(json["path"]["__type"], "nav_msgs::msg::Path");
+  ASSERT_EQ(json["path"]["header"]["__type"], "std_msgs::msg::Header");
+  ASSERT_EQ(json["path"]["header"]["stamp"]["__type"], "builtin_interfaces::msg::Time");
+  ASSERT_EQ(json["path"]["header"]["stamp"]["sec"], 1);
+  ASSERT_EQ(json["path"]["header"]["stamp"]["nanosec"], 2);
+  ASSERT_EQ(json["path"]["header"]["frame_id"], "map");
+  ASSERT_EQ(json["path"]["poses"][0]["__type"], "geometry_msgs::msg::PoseStamped");
+  ASSERT_EQ(json["path"]["poses"][0]["header"]["__type"], "std_msgs::msg::Header");
+  ASSERT_EQ(
+    json["path"]["poses"][0]["header"]["stamp"]["__type"],
+    "builtin_interfaces::msg::Time");
+  ASSERT_EQ(json["path"]["poses"][0]["header"]["stamp"]["sec"], 3);
+  ASSERT_EQ(json["path"]["poses"][0]["header"]["stamp"]["nanosec"], 4);
+  ASSERT_EQ(json["path"]["poses"][0]["header"]["frame_id"], "map");
+  ASSERT_EQ(json["path"]["poses"][0]["pose"]["__type"], "geometry_msgs::msg::Pose");
+  ASSERT_EQ(
+    json["path"]["poses"][0]["pose"]["position"]["__type"],
+    "geometry_msgs::msg::Point");
+  ASSERT_EQ(json["path"]["poses"][0]["pose"]["position"]["x"], 5.0);
+  ASSERT_EQ(json["path"]["poses"][0]["pose"]["position"]["y"], 6.0);
+  ASSERT_EQ(json["path"]["poses"][0]["pose"]["position"]["z"], 7.0);
+  ASSERT_EQ(
+    json["path"]["poses"][0]["pose"]["orientation"]["__type"],
+    "geometry_msgs::msg::Quaternion");
+  ASSERT_EQ(json["path"]["poses"][0]["pose"]["orientation"]["x"], 8.0);
+  ASSERT_EQ(json["path"]["poses"][0]["pose"]["orientation"]["y"], 9.0);
+  ASSERT_EQ(json["path"]["poses"][0]["pose"]["orientation"]["z"], 10.0);
+  ASSERT_EQ(json["path"]["poses"][0]["pose"]["orientation"]["w"], 11.0);
+  ASSERT_EQ(json["path"]["poses"][1]["__type"], "geometry_msgs::msg::PoseStamped");
+  ASSERT_EQ(json["path"]["poses"][1]["header"]["__type"], "std_msgs::msg::Header");
+  ASSERT_EQ(
+    json["path"]["poses"][1]["header"]["stamp"]["__type"],
+    "builtin_interfaces::msg::Time");
+  ASSERT_EQ(json["path"]["poses"][1]["header"]["stamp"]["sec"], 12);
+  ASSERT_EQ(json["path"]["poses"][1]["header"]["stamp"]["nanosec"], 13);
+  ASSERT_EQ(json["path"]["poses"][1]["header"]["frame_id"], "odom");
+  ASSERT_EQ(json["path"]["poses"][1]["pose"]["__type"], "geometry_msgs::msg::Pose");
+  ASSERT_EQ(
+    json["path"]["poses"][1]["pose"]["position"]["__type"],
+    "geometry_msgs::msg::Point");
+  ASSERT_EQ(json["path"]["poses"][1]["pose"]["position"]["x"], 14.0);
+  ASSERT_EQ(json["path"]["poses"][1]["pose"]["position"]["y"], 15.0);
+  ASSERT_EQ(json["path"]["poses"][1]["pose"]["position"]["z"], 16.0);
+  ASSERT_EQ(
+    json["path"]["poses"][1]["pose"]["orientation"]["__type"],
+    "geometry_msgs::msg::Quaternion");
+  ASSERT_EQ(json["path"]["poses"][1]["pose"]["orientation"]["x"], 17.0);
+  ASSERT_EQ(json["path"]["poses"][1]["pose"]["orientation"]["y"], 18.0);
+  ASSERT_EQ(json["path"]["poses"][1]["pose"]["orientation"]["z"], 19.0);
+  ASSERT_EQ(json["path"]["poses"][1]["pose"]["orientation"]["w"], 20.0);
+
+  // Check the two-ways transform, i.e. "from_json"
+  auto path_test2 = exporter.fromJson(json["path"])->first.cast<nav_msgs::msg::Path>();
+  ASSERT_EQ(path_test.header.stamp.sec, path_test2.header.stamp.sec);
+  ASSERT_EQ(path_test.header.stamp.nanosec, path_test2.header.stamp.nanosec);
+  ASSERT_EQ(path_test.header.frame_id, path_test2.header.frame_id);
+  ASSERT_EQ(path_test.poses[0].header.stamp.sec, path_test2.poses[0].header.stamp.sec);
+  ASSERT_EQ(path_test.poses[0].header.stamp.nanosec, path_test2.poses[0].header.stamp.nanosec);
+  ASSERT_EQ(path_test.poses[0].header.frame_id, path_test2.poses[0].header.frame_id);
+  ASSERT_EQ(path_test.poses[0].pose.position.x, path_test2.poses[0].pose.position.x);
+  ASSERT_EQ(path_test.poses[0].pose.position.y, path_test2.poses[0].pose.position.y);
+  ASSERT_EQ(path_test.poses[0].pose.position.z, path_test2.poses[0].pose.position.z);
+  ASSERT_EQ(path_test.poses[0].pose.orientation.x, path_test2.poses[0].pose.orientation.x);
+  ASSERT_EQ(path_test.poses[0].pose.orientation.y, path_test2.poses[0].pose.orientation.y);
+  ASSERT_EQ(path_test.poses[0].pose.orientation.z, path_test2.poses[0].pose.orientation.z);
+  ASSERT_EQ(path_test.poses[0].pose.orientation.w, path_test2.poses[0].pose.orientation.w);
+  ASSERT_EQ(path_test.poses[1].header.stamp.sec, path_test2.poses[1].header.stamp.sec);
+  ASSERT_EQ(path_test.poses[1].header.stamp.nanosec, path_test2.poses[1].header.stamp.nanosec);
+  ASSERT_EQ(path_test.poses[1].header.frame_id, path_test2.poses[1].header.frame_id);
+  ASSERT_EQ(path_test.poses[1].pose.position.x, path_test2.poses[1].pose.position.x);
+  ASSERT_EQ(path_test.poses[1].pose.position.y, path_test2.poses[1].pose.position.y);
+  ASSERT_EQ(path_test.poses[1].pose.position.z, path_test2.poses[1].pose.position.z);
+  ASSERT_EQ(path_test.poses[1].pose.orientation.x, path_test2.poses[1].pose.orientation.x);
+  ASSERT_EQ(path_test.poses[1].pose.orientation.y, path_test2.poses[1].pose.orientation.y);
+  ASSERT_EQ(path_test.poses[1].pose.orientation.z, path_test2.poses[1].pose.orientation.z);
+  ASSERT_EQ(path_test.poses[1].pose.orientation.w, path_test2.poses[1].pose.orientation.w);
+
+  // Convert from string
+  nav_msgs::msg::Path path_test3;
+  auto const test_json =
+    R"(json:
+      {
+        "__type": "nav_msgs::msg::Path",
+        "header": {
+          "__type": "std_msgs::msg::Header",
+          "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 1, "nanosec": 2},
+          "frame_id": "map"
+        },
+        "poses": [
+          {
+            "__type": "geometry_msgs::msg::PoseStamped",
+            "header": {
+              "__type": "std_msgs::msg::Header",
+              "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 3, "nanosec": 4},
+              "frame_id": "map"
+            },
+            "pose": {
+              "__type": "geometry_msgs::msg::Pose",
+              "position": {
+                "__type": "geometry_msgs::msg::Point",
+                "x": 5.0, "y": 6.0, "z": 7.0
+              },
+              "orientation": {
+                "__type": "geometry_msgs::msg::Quaternion",
+                "x": 8.0, "y": 9.0, "z": 10.0, "w": 11.0
+              }
+            }
+          },
+          {
+            "__type": "geometry_msgs::msg::PoseStamped",
+            "header": {
+              "__type": "std_msgs::msg::Header",
+              "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 12, "nanosec": 13},
+              "frame_id": "odom"
+            },
+            "pose": {
+              "__type": "geometry_msgs::msg::Pose",
+              "position": {
+                "__type": "geometry_msgs::msg::Point",
+                "x": 14.0, "y": 15.0, "z": 16.0
+              },
+              "orientation": {
+                "__type": "geometry_msgs::msg::Quaternion",
+                "x": 17.0, "y": 18.0, "z": 19.0, "w": 20.0
+              }
+            }
+          }
+        ]
+      }
+    )";
+  ASSERT_NO_THROW(
+    path_test3 =
+    BT::convertFromString<nav_msgs::msg::Path>(test_json));
+  ASSERT_EQ(path_test.header, path_test3.header);
+  ASSERT_EQ(path_test.poses[0].header, path_test3.poses[0].header);
+  ASSERT_EQ(path_test.poses[0].pose.position, path_test3.poses[0].pose.position);
+  ASSERT_EQ(path_test.poses[0].pose.orientation, path_test3.poses[0].pose.orientation);
+  ASSERT_EQ(path_test.poses[1].header, path_test3.poses[1].header);
+  ASSERT_EQ(path_test.poses[1].pose.position, path_test3.poses[1].pose.position);
+  ASSERT_EQ(path_test.poses[1].pose.orientation, path_test3.poses[1].pose.orientation);
+}
+
+TEST_F(JsonTest, test_waypoint_status)
+{
+  BT::JsonExporter & exporter = BT::JsonExporter::get();
+
+  nav2_msgs::msg::WaypointStatus waypoint_status_test;
+  waypoint_status_test.waypoint_status = 1;
+  waypoint_status_test.waypoint_index = 2;
+  waypoint_status_test.waypoint_pose.header.stamp.sec = 1;
+  waypoint_status_test.waypoint_pose.header.stamp.nanosec = 2;
+  waypoint_status_test.waypoint_pose.header.frame_id = "map";
+  waypoint_status_test.waypoint_pose.pose.position.x = 3.0;
+  waypoint_status_test.waypoint_pose.pose.position.y = 4.0;
+  waypoint_status_test.waypoint_pose.pose.position.z = 5.0;
+  waypoint_status_test.waypoint_pose.pose.orientation.x = 6.0;
+  waypoint_status_test.waypoint_pose.pose.orientation.y = 7.0;
+  waypoint_status_test.waypoint_pose.pose.orientation.z = 8.0;
+  waypoint_status_test.waypoint_pose.pose.orientation.w = 9.0;
+  waypoint_status_test.error_code = 10;
+  waypoint_status_test.error_msg = "error";
+
+  nlohmann::json json;
+  exporter.toJson(BT::Any(waypoint_status_test), json["waypoint_status"]);
+
+  std::cout << json.dump(2) << std::endl;
+
+  ASSERT_EQ(json["waypoint_status"]["__type"], "nav2_msgs::msg::WaypointStatus");
+  ASSERT_EQ(json["waypoint_status"]["waypoint_status"], 1);
+  ASSERT_EQ(json["waypoint_status"]["waypoint_index"], 2);
+  ASSERT_EQ(
+    json["waypoint_status"]["waypoint_pose"]["__type"],
+    "geometry_msgs::msg::PoseStamped");
+  ASSERT_EQ(
+    json["waypoint_status"]["waypoint_pose"]["header"]["__type"],
+    "std_msgs::msg::Header");
+  ASSERT_EQ(
+    json["waypoint_status"]["waypoint_pose"]["header"]["stamp"]["__type"],
+    "builtin_interfaces::msg::Time");
+  ASSERT_EQ(json["waypoint_status"]["waypoint_pose"]["header"]["stamp"]["sec"], 1);
+  ASSERT_EQ(json["waypoint_status"]["waypoint_pose"]["header"]["stamp"]["nanosec"], 2);
+  ASSERT_EQ(json["waypoint_status"]["waypoint_pose"]["header"]["frame_id"], "map");
+  ASSERT_EQ(
+    json["waypoint_status"]["waypoint_pose"]["pose"]["__type"],
+    "geometry_msgs::msg::Pose");
+  ASSERT_EQ(
+    json["waypoint_status"]["waypoint_pose"]["pose"]["position"]["__type"],
+    "geometry_msgs::msg::Point");
+  ASSERT_EQ(json["waypoint_status"]["waypoint_pose"]["pose"]["position"]["x"], 3.0);
+  ASSERT_EQ(json["waypoint_status"]["waypoint_pose"]["pose"]["position"]["y"], 4.0);
+  ASSERT_EQ(json["waypoint_status"]["waypoint_pose"]["pose"]["position"]["z"], 5.0);
+  ASSERT_EQ(
+    json["waypoint_status"]["waypoint_pose"]["pose"]["orientation"]["__type"],
+    "geometry_msgs::msg::Quaternion");
+  ASSERT_EQ(json["waypoint_status"]["waypoint_pose"]["pose"]["orientation"]["x"], 6.0);
+  ASSERT_EQ(json["waypoint_status"]["waypoint_pose"]["pose"]["orientation"]["y"], 7.0);
+  ASSERT_EQ(json["waypoint_status"]["waypoint_pose"]["pose"]["orientation"]["z"], 8.0);
+  ASSERT_EQ(json["waypoint_status"]["waypoint_pose"]["pose"]["orientation"]["w"], 9.0);
+  ASSERT_EQ(json["waypoint_status"]["error_code"], 10);
+  ASSERT_EQ(json["waypoint_status"]["error_msg"], "error");
+
+  // Check the two-ways transform, i.e. "from_json"
+  auto waypoint_status_test2 =
+    exporter.fromJson(json["waypoint_status"])->first.cast<nav2_msgs::msg::WaypointStatus>();
+  ASSERT_EQ(waypoint_status_test.waypoint_status, waypoint_status_test2.waypoint_status);
+  ASSERT_EQ(waypoint_status_test.waypoint_index, waypoint_status_test2.waypoint_index);
+  ASSERT_EQ(
+    waypoint_status_test.waypoint_pose.header.stamp.sec,
+    waypoint_status_test2.waypoint_pose.header.stamp.sec);
+  ASSERT_EQ(
+    waypoint_status_test.waypoint_pose.header.stamp.nanosec,
+    waypoint_status_test2.waypoint_pose.header.stamp.nanosec);
+  ASSERT_EQ(
+    waypoint_status_test.waypoint_pose.header.frame_id,
+    waypoint_status_test2.waypoint_pose.header.frame_id);
+  ASSERT_EQ(
+    waypoint_status_test.waypoint_pose.pose.position.x,
+    waypoint_status_test2.waypoint_pose.pose.position.x);
+  ASSERT_EQ(
+    waypoint_status_test.waypoint_pose.pose.position.y,
+    waypoint_status_test2.waypoint_pose.pose.position.y);
+  ASSERT_EQ(
+    waypoint_status_test.waypoint_pose.pose.position.z,
+    waypoint_status_test2.waypoint_pose.pose.position.z);
+  ASSERT_EQ(
+    waypoint_status_test.waypoint_pose.pose.orientation.x,
+    waypoint_status_test2.waypoint_pose.pose.orientation.x);
+  ASSERT_EQ(
+    waypoint_status_test.waypoint_pose.pose.orientation.y,
+    waypoint_status_test2.waypoint_pose.pose.orientation.y);
+  ASSERT_EQ(
+    waypoint_status_test.waypoint_pose.pose.orientation.z,
+    waypoint_status_test2.waypoint_pose.pose.orientation.z);
+  ASSERT_EQ(
+    waypoint_status_test.waypoint_pose.pose.orientation.w,
+    waypoint_status_test2.waypoint_pose.pose.orientation.w);
+  ASSERT_EQ(waypoint_status_test.error_code, waypoint_status_test2.error_code);
+  ASSERT_EQ(waypoint_status_test.error_msg, waypoint_status_test2.error_msg);
+
+  // Convert from string
+  nav2_msgs::msg::WaypointStatus waypoint_status_test3;
+  auto const test_json =
+    R"(json:
+      {
+        "__type": "nav2_msgs::msg::WaypointStatus",
+        "waypoint_status": 1,
+        "waypoint_index": 2,
+        "waypoint_pose": {
+          "__type": "geometry_msgs::msg::PoseStamped",
+          "header": {
+            "__type": "std_msgs::msg::Header",
+            "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 1, "nanosec": 2},
+            "frame_id": "map"
+          },
+          "pose": {
+            "__type": "geometry_msgs::msg::Pose",
+            "position": {
+              "__type": "geometry_msgs::msg::Point",
+              "x": 3.0, "y": 4.0, "z": 5.0
+            },
+            "orientation": {
+              "__type": "geometry_msgs::msg::Quaternion",
+              "x": 6.0, "y": 7.0, "z": 8.0, "w": 9.0
+            }
+          }
+        },
+        "error_code": 10,
+        "error_msg": "error"
+      }
+    )";
+  ASSERT_NO_THROW(
+    waypoint_status_test3 =
+    BT::convertFromString<nav2_msgs::msg::WaypointStatus>(test_json));
+  ASSERT_EQ(waypoint_status_test.waypoint_status, waypoint_status_test3.waypoint_status);
+  ASSERT_EQ(waypoint_status_test.waypoint_index, waypoint_status_test3.waypoint_index);
+  ASSERT_EQ(waypoint_status_test.waypoint_pose, waypoint_status_test3.waypoint_pose);
+  ASSERT_EQ(waypoint_status_test.error_code, waypoint_status_test3.error_code);
+  ASSERT_EQ(waypoint_status_test.error_msg, waypoint_status_test3.error_msg);
+}
+
+TEST_F(JsonTest, test_waypoint_status_vector)
+{
+  BT::JsonExporter & exporter = BT::JsonExporter::get();
+
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_status_vector_test;
+  waypoint_status_vector_test.resize(2);
+  waypoint_status_vector_test[0].waypoint_status = 1;
+  waypoint_status_vector_test[0].waypoint_index = 2;
+  waypoint_status_vector_test[0].waypoint_pose.header.stamp.sec = 1;
+  waypoint_status_vector_test[0].waypoint_pose.header.stamp.nanosec = 2;
+  waypoint_status_vector_test[0].waypoint_pose.header.frame_id = "map";
+  waypoint_status_vector_test[0].waypoint_pose.pose.position.x = 3.0;
+  waypoint_status_vector_test[0].waypoint_pose.pose.position.y = 4.0;
+  waypoint_status_vector_test[0].waypoint_pose.pose.position.z = 5.0;
+  waypoint_status_vector_test[0].waypoint_pose.pose.orientation.x = 6.0;
+  waypoint_status_vector_test[0].waypoint_pose.pose.orientation.y = 7.0;
+  waypoint_status_vector_test[0].waypoint_pose.pose.orientation.z = 8.0;
+  waypoint_status_vector_test[0].waypoint_pose.pose.orientation.w = 9.0;
+  waypoint_status_vector_test[0].error_code = 10;
+  waypoint_status_vector_test[0].error_msg = "error";
+
+  waypoint_status_vector_test[1].waypoint_status = 11;
+  waypoint_status_vector_test[1].waypoint_index = 12;
+  waypoint_status_vector_test[1].waypoint_pose.header.stamp.sec = 13;
+  waypoint_status_vector_test[1].waypoint_pose.header.stamp.nanosec = 14;
+  waypoint_status_vector_test[1].waypoint_pose.header.frame_id = "odom";
+  waypoint_status_vector_test[1].waypoint_pose.pose.position.x = 15.0;
+  waypoint_status_vector_test[1].waypoint_pose.pose.position.y = 16.0;
+  waypoint_status_vector_test[1].waypoint_pose.pose.position.z = 17.0;
+  waypoint_status_vector_test[1].waypoint_pose.pose.orientation.x = 18.0;
+  waypoint_status_vector_test[1].waypoint_pose.pose.orientation.y = 19.0;
+  waypoint_status_vector_test[1].waypoint_pose.pose.orientation.z = 20.0;
+  waypoint_status_vector_test[1].waypoint_pose.pose.orientation.w = 21.0;
+  waypoint_status_vector_test[1].error_code = 22;
+  waypoint_status_vector_test[1].error_msg = "error2";
+
+  nlohmann::json json;
+  exporter.toJson(BT::Any(waypoint_status_vector_test), json["waypoint_status_vector"]);
+  std::cout << json.dump(2) << std::endl;
+
+  ASSERT_EQ(json["waypoint_status_vector"][0]["__type"], "nav2_msgs::msg::WaypointStatus");
+  ASSERT_EQ(json["waypoint_status_vector"][0]["waypoint_status"], 1);
+  ASSERT_EQ(json["waypoint_status_vector"][0]["waypoint_index"], 2);
+  ASSERT_EQ(
+    json["waypoint_status_vector"][0]["waypoint_pose"]["__type"],
+    "geometry_msgs::msg::PoseStamped");
+  ASSERT_EQ(
+    json["waypoint_status_vector"][0]["waypoint_pose"]["header"]["__type"],
+    "std_msgs::msg::Header");
+  ASSERT_EQ(
+    json["waypoint_status_vector"][0]["waypoint_pose"]["header"]["stamp"]["__type"],
+    "builtin_interfaces::msg::Time");
+  ASSERT_EQ(json["waypoint_status_vector"][0]["waypoint_pose"]["header"]["stamp"]["sec"], 1);
+  ASSERT_EQ(json["waypoint_status_vector"][0]["waypoint_pose"]["header"]["stamp"]["nanosec"], 2);
+  ASSERT_EQ(json["waypoint_status_vector"][0]["waypoint_pose"]["header"]["frame_id"], "map");
+  ASSERT_EQ(
+    json["waypoint_status_vector"][0]["waypoint_pose"]["pose"]["__type"],
+    "geometry_msgs::msg::Pose");
+  ASSERT_EQ(
+    json["waypoint_status_vector"][0]["waypoint_pose"]["pose"]["position"]["__type"],
+    "geometry_msgs::msg::Point");
+  ASSERT_EQ(json["waypoint_status_vector"][0]["waypoint_pose"]["pose"]["position"]["x"], 3.0);
+  ASSERT_EQ(json["waypoint_status_vector"][0]["waypoint_pose"]["pose"]["position"]["y"], 4.0);
+  ASSERT_EQ(json["waypoint_status_vector"][0]["waypoint_pose"]["pose"]["position"]["z"], 5.0);
+  ASSERT_EQ(
+    json["waypoint_status_vector"][0]["waypoint_pose"]["pose"]["orientation"]["__type"],
+    "geometry_msgs::msg::Quaternion");
+  ASSERT_EQ(json["waypoint_status_vector"][0]["waypoint_pose"]["pose"]["orientation"]["x"], 6.0);
+  ASSERT_EQ(json["waypoint_status_vector"][0]["waypoint_pose"]["pose"]["orientation"]["y"], 7.0);
+  ASSERT_EQ(json["waypoint_status_vector"][0]["waypoint_pose"]["pose"]["orientation"]["z"], 8.0);
+  ASSERT_EQ(json["waypoint_status_vector"][0]["waypoint_pose"]["pose"]["orientation"]["w"], 9.0);
+  ASSERT_EQ(json["waypoint_status_vector"][0]["error_code"], 10);
+  ASSERT_EQ(json["waypoint_status_vector"][0]["error_msg"], "error");
+  ASSERT_EQ(json["waypoint_status_vector"][1]["__type"], "nav2_msgs::msg::WaypointStatus");
+  ASSERT_EQ(json["waypoint_status_vector"][1]["waypoint_status"], 11);
+  ASSERT_EQ(json["waypoint_status_vector"][1]["waypoint_index"], 12);
+  ASSERT_EQ(
+    json["waypoint_status_vector"][1]["waypoint_pose"]["__type"],
+    "geometry_msgs::msg::PoseStamped");
+  ASSERT_EQ(
+    json["waypoint_status_vector"][1]["waypoint_pose"]["header"]["__type"],
+    "std_msgs::msg::Header");
+  ASSERT_EQ(
+    json["waypoint_status_vector"][1]["waypoint_pose"]["header"]["stamp"]["__type"],
+    "builtin_interfaces::msg::Time");
+  ASSERT_EQ(json["waypoint_status_vector"][1]["waypoint_pose"]["header"]["stamp"]["sec"], 13);
+  ASSERT_EQ(json["waypoint_status_vector"][1]["waypoint_pose"]["header"]["stamp"]["nanosec"], 14);
+  ASSERT_EQ(json["waypoint_status_vector"][1]["waypoint_pose"]["header"]["frame_id"], "odom");
+  ASSERT_EQ(
+    json["waypoint_status_vector"][1]["waypoint_pose"]["pose"]["__type"],
+    "geometry_msgs::msg::Pose");
+  ASSERT_EQ(
+    json["waypoint_status_vector"][1]["waypoint_pose"]["pose"]["position"]["__type"],
+    "geometry_msgs::msg::Point");
+  ASSERT_EQ(json["waypoint_status_vector"][1]["waypoint_pose"]["pose"]["position"]["x"], 15.0);
+  ASSERT_EQ(json["waypoint_status_vector"][1]["waypoint_pose"]["pose"]["position"]["y"], 16.0);
+  ASSERT_EQ(json["waypoint_status_vector"][1]["waypoint_pose"]["pose"]["position"]["z"], 17.0);
+  ASSERT_EQ(
+    json["waypoint_status_vector"][1]["waypoint_pose"]["pose"]["orientation"]["__type"],
+    "geometry_msgs::msg::Quaternion");
+  ASSERT_EQ(json["waypoint_status_vector"][1]["waypoint_pose"]["pose"]["orientation"]["x"], 18.0);
+  ASSERT_EQ(json["waypoint_status_vector"][1]["waypoint_pose"]["pose"]["orientation"]["y"], 19.0);
+  ASSERT_EQ(json["waypoint_status_vector"][1]["waypoint_pose"]["pose"]["orientation"]["z"], 20.0);
+  ASSERT_EQ(json["waypoint_status_vector"][1]["waypoint_pose"]["pose"]["orientation"]["w"], 21.0);
+  ASSERT_EQ(json["waypoint_status_vector"][1]["error_code"], 22);
+  ASSERT_EQ(json["waypoint_status_vector"][1]["error_msg"], "error2");
+
+  // Check the two-ways transform, i.e. "from_json"
+  // auto waypoint_status_vector_test2 =
+  //   exporter.fromJson(json["waypoint_status_vector"])->first.cast<std::vector<nav2_msgs::msg::WaypointStatus>>();
+}
diff --git a/nav2_behaviors/README.md b/nav2_behaviors/README.md
index be0ee32461f..2b91d9752bb 100644
--- a/nav2_behaviors/README.md
+++ b/nav2_behaviors/README.md
@@ -15,4 +15,4 @@ See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/
 See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins.
 
 The `TimedBehavior` template makes use of a [nav2_util::TwistPublisher](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).
-The `AssistedTeleop` behavior makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).
\ No newline at end of file
+The `AssistedTeleop` behavior makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).
diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp
index 102b61a0d16..048bfc079ff 100644
--- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp
+++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp
@@ -266,7 +266,8 @@ class DriveOnHeading : public TimedBehavior<ActionT>
     node->get_parameter(this->behavior_name_ + ".deceleration_limit", deceleration_limit_);
     node->get_parameter(this->behavior_name_ + ".minimum_speed", minimum_speed_);
     if (acceleration_limit_ <= 0.0 || deceleration_limit_ >= 0.0) {
-      RCLCPP_ERROR(this->logger_,
+      RCLCPP_ERROR(
+        this->logger_,
         "DriveOnHeading: acceleration_limit and deceleration_limit must be "
         "positive and negative respectively");
       acceleration_limit_ = std::abs(acceleration_limit_);
diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
index c8b11a2452c..987575354bd 100644
--- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
+++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
@@ -227,7 +227,8 @@ class TimedBehavior : public nav2_core::Behavior
     if (on_run_result.status != Status::SUCCEEDED) {
       result->error_code = on_run_result.error_code;
       result->error_msg = on_run_result.error_msg;
-      RCLCPP_INFO(logger_, "Initial checks failed for %s - %s", behavior_name_.c_str(),
+      RCLCPP_INFO(
+        logger_, "Initial checks failed for %s - %s", behavior_name_.c_str(),
         on_run_result.error_msg.c_str());
       action_server_->terminate_current(result);
       return;
diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py
index a5f6b9c3dc4..8efd9b490f9 100644
--- a/nav2_bringup/launch/bringup_launch.py
+++ b/nav2_bringup/launch/bringup_launch.py
@@ -15,24 +15,18 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-    GroupAction,
-    IncludeLaunchDescription,
-    SetEnvironmentVariable,
-)
+from launch.actions import (DeclareLaunchArgument, GroupAction, IncludeLaunchDescription,
+                            SetEnvironmentVariable)
 from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PythonExpression
-from launch_ros.actions import Node
-from launch_ros.actions import PushROSNamespace
+from launch_ros.actions import Node, PushROSNamespace
 from launch_ros.descriptions import ParameterFile
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     # Get the launch directory
     bringup_dir = get_package_share_directory('nav2_bringup')
     launch_dir = os.path.join(bringup_dir, 'launch')
diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py
index 823f56ba2a7..2bd90b5262e 100644
--- a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py
+++ b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py
@@ -20,24 +20,14 @@
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    ForEach,
-    GroupAction,
-    IncludeLaunchDescription,
-    LogInfo,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
-
+from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
+                            ForEach, GroupAction, IncludeLaunchDescription, LogInfo,
+                            OpaqueFunction, RegisterEventHandler)
 from launch.conditions import IfCondition
 from launch.event_handlers import OnShutdown
 from launch.launch_context import LaunchContext
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, TextSubstitution
-
 import yaml
 
 
@@ -59,7 +49,7 @@ def count_robots(context: LaunchContext) -> list[LogInfo]:
     return [LogInfo(msg=[log_msg])]
 
 
-def generate_robot_actions(name: str = '', pose: dict = {}) -> list[GroupAction]:
+def generate_robot_actions(name: str = '', pose: dict[str, float] = {}) -> list[GroupAction]:
     """Generate the actions to launch a robot with the given name and pose."""
     bringup_dir = get_package_share_directory('nav2_bringup')
     launch_dir = os.path.join(bringup_dir, 'launch')
@@ -114,7 +104,7 @@ def generate_robot_actions(name: str = '', pose: dict = {}) -> list[GroupAction]
     return [group]
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     """
     Bring up the multi-robots with given launch arguments.
 
diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py
index 909b81eb3e1..9a7fded30c2 100644
--- a/nav2_bringup/launch/localization_launch.py
+++ b/nav2_bringup/launch/localization_launch.py
@@ -15,21 +15,17 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, GroupAction
-from launch.actions import SetEnvironmentVariable
+from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
 from launch.conditions import IfCondition
-from launch.substitutions import EqualsSubstitution
-from launch.substitutions import LaunchConfiguration, PythonExpression
-from launch.substitutions import NotEqualsSubstitution
-from launch_ros.actions import LoadComposableNodes, SetParameter
-from launch_ros.actions import Node
+from launch.substitutions import (EqualsSubstitution, LaunchConfiguration, NotEqualsSubstitution,
+                                  PythonExpression)
+from launch_ros.actions import LoadComposableNodes, Node, SetParameter
 from launch_ros.descriptions import ComposableNode, ParameterFile
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     # Get the launch directory
     bringup_dir = get_package_share_directory('nav2_bringup')
 
diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py
index 987c398992c..31c543cbeb8 100644
--- a/nav2_bringup/launch/navigation_launch.py
+++ b/nav2_bringup/launch/navigation_launch.py
@@ -15,18 +15,16 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
 from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration, PythonExpression
-from launch_ros.actions import LoadComposableNodes, SetParameter
-from launch_ros.actions import Node
+from launch_ros.actions import LoadComposableNodes, Node, SetParameter
 from launch_ros.descriptions import ComposableNode, ParameterFile
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     # Get the launch directory
     bringup_dir = get_package_share_directory('nav2_bringup')
 
diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py
index 425dac4edee..8d5e3e154c0 100644
--- a/nav2_bringup/launch/rviz_launch.py
+++ b/nav2_bringup/launch/rviz_launch.py
@@ -15,7 +15,6 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler
 from launch.event_handlers import OnProcessExit
@@ -24,7 +23,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     # Get the launch directory
     bringup_dir = get_package_share_directory('nav2_bringup')
 
diff --git a/nav2_bringup/launch/slam_launch.py b/nav2_bringup/launch/slam_launch.py
index f9294b711d2..21070e91cc4 100644
--- a/nav2_bringup/launch/slam_launch.py
+++ b/nav2_bringup/launch/slam_launch.py
@@ -15,7 +15,6 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription
 from launch.conditions import IfCondition, UnlessCondition
@@ -26,7 +25,7 @@
 from nav2_common.launch import HasNodeParams, RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     # Input parameters declaration
     namespace = LaunchConfiguration('namespace')
     params_file = LaunchConfiguration('params_file')
diff --git a/nav2_bringup/launch/tb3_loopback_simulation.launch.py b/nav2_bringup/launch/tb3_loopback_simulation_launch.py
similarity index 98%
rename from nav2_bringup/launch/tb3_loopback_simulation.launch.py
rename to nav2_bringup/launch/tb3_loopback_simulation_launch.py
index 3553852597c..8e1f6176568 100644
--- a/nav2_bringup/launch/tb3_loopback_simulation.launch.py
+++ b/nav2_bringup/launch/tb3_loopback_simulation_launch.py
@@ -17,23 +17,17 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-    GroupAction,
-    IncludeLaunchDescription,
-)
+from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription
 from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node, SetParameter
 from launch_ros.descriptions import ParameterFile
-
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     # Get the launch directory
     bringup_dir = get_package_share_directory('nav2_bringup')
     loopback_sim_dir = get_package_share_directory('nav2_loopback_sim')
diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py
index 5f8659f95b7..1a9698ba149 100644
--- a/nav2_bringup/launch/tb3_simulation_launch.py
+++ b/nav2_bringup/launch/tb3_simulation_launch.py
@@ -18,24 +18,17 @@
 import tempfile
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
+from launch.actions import (DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription,
+                            OpaqueFunction, RegisterEventHandler)
 from launch.conditions import IfCondition
 from launch.event_handlers import OnShutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PythonExpression
-
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     # Get the launch directory
     bringup_dir = get_package_share_directory('nav2_bringup')
     launch_dir = os.path.join(bringup_dir, 'launch')
diff --git a/nav2_bringup/launch/tb4_loopback_simulation.launch.py b/nav2_bringup/launch/tb4_loopback_simulation_launch.py
similarity index 98%
rename from nav2_bringup/launch/tb4_loopback_simulation.launch.py
rename to nav2_bringup/launch/tb4_loopback_simulation_launch.py
index 5373efbfab3..07866736123 100644
--- a/nav2_bringup/launch/tb4_loopback_simulation.launch.py
+++ b/nav2_bringup/launch/tb4_loopback_simulation_launch.py
@@ -17,23 +17,17 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-    GroupAction,
-    IncludeLaunchDescription,
-)
+from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription
 from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import Command, LaunchConfiguration
 from launch_ros.actions import Node, SetParameter
 from launch_ros.descriptions import ParameterFile
-
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     # Get the launch directory
     bringup_dir = get_package_share_directory('nav2_bringup')
     loopback_sim_dir = get_package_share_directory('nav2_loopback_sim')
diff --git a/nav2_bringup/launch/tb4_simulation_launch.py b/nav2_bringup/launch/tb4_simulation_launch.py
index 5930a1b73b8..5bc0db49835 100644
--- a/nav2_bringup/launch/tb4_simulation_launch.py
+++ b/nav2_bringup/launch/tb4_simulation_launch.py
@@ -18,25 +18,17 @@
 import tempfile
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
+from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
+                            IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler)
 from launch.conditions import IfCondition
 from launch.event_handlers import OnShutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import Command, LaunchConfiguration, PythonExpression
-
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     # Get the launch directory
     bringup_dir = get_package_share_directory('nav2_bringup')
     launch_dir = os.path.join(bringup_dir, 'launch')
diff --git a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py
index 862a0083799..e0cdc96f9d4 100644
--- a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py
+++ b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py
@@ -25,25 +25,17 @@
 import tempfile
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    GroupAction,
-    IncludeLaunchDescription,
-    LogInfo,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
+from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
+                            GroupAction, IncludeLaunchDescription, LogInfo, OpaqueFunction,
+                            RegisterEventHandler)
 from launch.conditions import IfCondition
 from launch.event_handlers import OnShutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, TextSubstitution
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     # Get the launch directory
     bringup_dir = get_package_share_directory('nav2_bringup')
     launch_dir = os.path.join(bringup_dir, 'launch')
diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
index 04021ac6d52..0914e5cbe03 100644
--- a/nav2_bringup/params/nav2_params.yaml
+++ b/nav2_bringup/params/nav2_params.yaml
@@ -30,6 +30,7 @@ amcl:
     sigma_hit: 0.2
     tf_broadcast: true
     transform_tolerance: 1.0
+    service_introspection_mode: "disabled"
     update_min_a: 0.2
     update_min_d: 0.25
     z_hit: 0.5
@@ -48,11 +49,16 @@ bt_navigator:
     default_server_timeout: 20
     wait_for_service_timeout: 1000
     action_server_result_timeout: 900.0
+    service_introspection_mode: "disabled"
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
+      enable_groot_monitoring: false
+      groot_server_port: 1667
     navigate_through_poses:
       plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
+      enable_groot_monitoring: false
+      groot_server_port: 1669
     # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
@@ -113,6 +119,7 @@ controller_server:
       ax_max: 3.0
       ax_min: -3.0
       ay_max: 3.0
+      ay_min: -3.0
       az_max: 3.5
       vx_std: 0.2
       vy_std: 0.2
@@ -128,16 +135,24 @@ controller_server:
       gamma: 0.015
       motion_model: "DiffDrive"
       visualize: true
+      publish_optimal_trajectory: true
       regenerate_noises: true
       TrajectoryVisualizer:
         trajectory_step: 5
         time_step: 3
       AckermannConstraints:
         min_turning_r: 0.2
-      critics: [
-        "ConstraintCritic", "CostCritic", "GoalCritic",
-        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
-        "PathAngleCritic", "PreferForwardCritic"]
+      critics:
+        [
+          "ConstraintCritic",
+          "CostCritic",
+          "GoalCritic",
+          "GoalAngleCritic",
+          "PathAlignCritic",
+          "PathFollowCritic",
+          "PathAngleCritic",
+          "PreferForwardCritic",
+        ]
       ConstraintCritic:
         enabled: true
         cost_power: 1
@@ -161,8 +176,9 @@ controller_server:
         enabled: true
         cost_power: 1
         cost_weight: 3.81
+        near_collision_cost: 253
         critical_cost: 300.0
-        consider_footprint: true
+        consider_footprint: false
         collision_cost: 1000000.0
         near_goal_distance: 1.0
         trajectory_point_step: 2
@@ -241,6 +257,7 @@ local_costmap:
         plugin: "nav2_costmap_2d::StaticLayer"
         map_subscribe_transient_local: True
       always_send_full_costmap: True
+      service_introspection_mode: "disabled"
 
 global_costmap:
   global_costmap:
@@ -281,13 +298,15 @@ global_costmap:
         cost_scaling_factor: 3.0
         inflation_radius: 0.7
       always_send_full_costmap: True
+      service_introspection_mode: "disabled"
 
 # The yaml_filename does not need to be specified since it going to be set by defaults in launch.
 # If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py
 # file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used.
-# map_server:
-#   ros__parameters:
-#     yaml_filename: ""
+map_server:
+  ros__parameters:
+    # yaml_filename: ""
+    service_introspection_mode: "disabled"
 
 map_saver:
   ros__parameters:
@@ -295,12 +314,14 @@ map_saver:
     free_thresh_default: 0.25
     occupied_thresh_default: 0.65
     map_subscribe_transient_local: True
+    service_introspection_mode: "disabled"
 
 planner_server:
   ros__parameters:
     expected_planner_frequency: 20.0
     planner_plugins: ["GridBased"]
     costmap_update_timeout: 1.0
+    service_introspection_mode: "disabled"
     GridBased:
       plugin: "nav2_navfn_planner::NavfnPlanner"
       tolerance: 0.5
@@ -323,7 +344,8 @@ behavior_server:
     local_footprint_topic: local_costmap/published_footprint
     global_footprint_topic: global_costmap/published_footprint
     cycle_frequency: 10.0
-    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
+    behavior_plugins:
+      ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
     spin:
       plugin: "nav2_behaviors::Spin"
     backup:
@@ -353,6 +375,7 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
+    service_introspection_mode: "disabled"
     action_server_result_timeout: 900.0
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
@@ -416,18 +439,19 @@ docking_server:
     max_retries: 3
     base_frame: "base_link"
     fixed_frame: "odom"
-    dock_backwards: false
     dock_prestaging_tolerance: 0.5
+    service_introspection_mode: "disabled"
 
     # Types of docks
-    dock_plugins: ['simple_charging_dock']
+    dock_plugins: ["simple_charging_dock"]
     simple_charging_dock:
-      plugin: 'opennav_docking::SimpleChargingDock'
+      plugin: "opennav_docking::SimpleChargingDock"
       docking_threshold: 0.05
       staging_x_offset: -0.7
       use_external_detection_pose: true
       use_battery_status: false # true
       use_stall_detection: false # true
+      dock_direction: "forward"
 
       external_detection_timeout: 1.0
       external_detection_translation_x: -0.18
@@ -451,8 +475,8 @@ docking_server:
       v_linear_min: 0.15
       v_linear_max: 0.15
       use_collision_detection: true
-      costmap_topic: "/local_costmap/costmap_raw"
-      footprint_topic: "/local_costmap/published_footprint"
+      costmap_topic: "local_costmap/costmap_raw"
+      footprint_topic: "local_costmap/published_footprint"
       transform_tolerance: 0.1
       projection_time: 5.0
       simulation_step: 0.1
@@ -463,5 +487,5 @@ loopback_simulator:
     base_frame_id: "base_footprint"
     odom_frame_id: "odom"
     map_frame_id: "map"
-    scan_frame_id: "base_scan"  # tb4_loopback_simulator.launch.py remaps to 'rplidar_link'
+    scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link'
     update_duration: 0.02
diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
index e7a60c5600c..6c32906f4f9 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
+++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
@@ -14,7 +14,7 @@
         <RateController hz="0.333">
           <RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
             <ReactiveSequence>
-              <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
+              <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
               <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
             </ReactiveSequence>
             <Sequence>
diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp
index de68270e29b..2226c5fc3d7 100644
--- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp
+++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp
@@ -23,6 +23,7 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_core/behavior_tree_navigator.hpp"
 #include "nav2_msgs/action/navigate_through_poses.hpp"
+#include "nav2_msgs/msg/waypoint_status.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
@@ -109,6 +110,7 @@ class NavigateThroughPosesNavigator
   rclcpp::Time start_time_;
   std::string goals_blackboard_id_;
   std::string path_blackboard_id_;
+  std::string waypoint_statuses_blackboard_id_;
 
   // Odometry smoother object
   std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
index bcadbafa10c..5c7363783ac 100644
--- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
+++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
@@ -42,9 +42,28 @@ NavigateThroughPosesNavigator::configure(
 
   path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string();
 
+  if (!node->has_parameter("waypoint_statuses_blackboard_id")) {
+    node->declare_parameter("waypoint_statuses_blackboard_id", std::string("waypoint_statuses"));
+  }
+
+  waypoint_statuses_blackboard_id_ =
+    node->get_parameter("waypoint_statuses_blackboard_id").as_string();
+
   // Odometry smoother object for getting current speed
   odom_smoother_ = odom_smoother;
 
+  if (!node->has_parameter(getName() + ".enable_groot_monitoring")) {
+    node->declare_parameter(getName() + ".enable_groot_monitoring", false);
+  }
+
+  if (!node->has_parameter(getName() + ".groot_server_port")) {
+    node->declare_parameter(getName() + ".groot_server_port", 1669);
+  }
+
+  bt_action_server_->setGrootMonitoring(
+    node->get_parameter(getName() + ".enable_groot_monitoring").as_bool(),
+    node->get_parameter(getName() + ".groot_server_port").as_int());
+
   return true;
 }
 
@@ -75,7 +94,8 @@ NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
   auto bt_xml_filename = goal->behavior_tree;
 
   if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) {
-    bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+    bt_action_server_->setInternalError(
+      ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
       "Error loading XML file: " + bt_xml_filename + ". Navigation canceled.");
     return false;
   }
@@ -86,20 +106,38 @@ NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
 void
 NavigateThroughPosesNavigator::goalCompleted(
   typename ActionT::Result::SharedPtr result,
-  const nav2_behavior_tree::BtStatus /*final_bt_status*/)
+  const nav2_behavior_tree::BtStatus final_bt_status)
 {
   if (result->error_code == 0) {
     if (bt_action_server_->populateInternalError(result)) {
-      RCLCPP_WARN(logger_,
+      RCLCPP_WARN(
+        logger_,
         "NavigateThroughPosesNavigator::goalCompleted, internal error %d:'%s'.",
         result->error_code,
         result->error_msg.c_str());
     }
   } else {
-    RCLCPP_WARN(logger_, "NavigateThroughPosesNavigator::goalCompleted error %d:'%s'.",
+    RCLCPP_WARN(
+      logger_, "NavigateThroughPosesNavigator::goalCompleted error %d:'%s'.",
       result->error_code,
       result->error_msg.c_str());
   }
+
+  // populate waypoint statuses in result
+  auto blackboard = bt_action_server_->getBlackboard();
+  auto waypoint_statuses =
+    blackboard->get<std::vector<nav2_msgs::msg::WaypointStatus>>(waypoint_statuses_blackboard_id_);
+
+  // populate remaining waypoint statuses based on final_bt_status
+  auto integrate_waypoint_status = final_bt_status == nav2_behavior_tree::BtStatus::SUCCEEDED ?
+    nav2_msgs::msg::WaypointStatus::COMPLETED : nav2_msgs::msg::WaypointStatus::FAILED;
+  for (auto & waypoint_status : waypoint_statuses) {
+    if (waypoint_status.waypoint_status == nav2_msgs::msg::WaypointStatus::PENDING) {
+      waypoint_status.waypoint_status = integrate_waypoint_status;
+    }
+  }
+
+  result->waypoint_statuses = std::move(waypoint_statuses);
 }
 
 void
@@ -116,6 +154,9 @@ NavigateThroughPosesNavigator::onLoop()
   nav_msgs::msg::Goals goal_poses;
   [[maybe_unused]] auto res = blackboard->get(goals_blackboard_id_, goal_poses);
 
+  feedback_msg->waypoint_statuses =
+    blackboard->get<std::vector<nav2_msgs::msg::WaypointStatus>>(waypoint_statuses_blackboard_id_);
+
   if (goal_poses.goals.size() == 0) {
     bt_action_server_->publishFeedback(feedback_msg);
     return;
@@ -223,7 +264,8 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr
       feedback_utils_.global_frame, feedback_utils_.robot_frame,
       feedback_utils_.transform_tolerance))
   {
-    bt_action_server_->setInternalError(ActionT::Result::TF_ERROR,
+    bt_action_server_->setInternalError(
+      ActionT::Result::TF_ERROR,
       "Initial robot pose is not available.");
     return false;
   }
@@ -235,7 +277,8 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr
         goal_pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame,
         feedback_utils_.transform_tolerance))
     {
-      bt_action_server_->setInternalError(ActionT::Result::TF_ERROR,
+      bt_action_server_->setInternalError(
+        ActionT::Result::TF_ERROR,
         "Failed to transform a goal pose (" + std::to_string(i) + ") provided with frame_id '" +
         goal_pose.header.frame_id +
         "' to the global frame '" +
@@ -250,7 +293,7 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr
     RCLCPP_INFO(
       logger_, "Begin navigating from current location through %zu poses to (%.2f, %.2f)",
       goals_array.goals.size(), goals_array.goals.back().pose.position.x,
-        goals_array.goals.back().pose.position.y);
+      goals_array.goals.back().pose.position.y);
   }
 
   // Reset state for new action feedback
@@ -259,8 +302,19 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr
   blackboard->set("number_recoveries", 0);  // NOLINT
 
   // Update the goal pose on the blackboard
-  blackboard->set<nav_msgs::msg::Goals>(goals_blackboard_id_,
-      std::move(goals_array));
+  blackboard->set<nav_msgs::msg::Goals>(
+    goals_blackboard_id_,
+    std::move(goals_array));
+
+  // Reset the waypoint states vector in the blackboard
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses(goals_array.goals.size());
+  for (size_t waypoint_index = 0; waypoint_index < goals_array.goals.size(); ++waypoint_index) {
+    waypoint_statuses[waypoint_index].waypoint_index = waypoint_index;
+    waypoint_statuses[waypoint_index].waypoint_pose = goals_array.goals[waypoint_index];
+  }
+  blackboard->set<decltype(waypoint_statuses)>(
+    waypoint_statuses_blackboard_id_,
+    std::move(waypoint_statuses));
 
   return true;
 }
diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
index 87f7bd2f571..139f473080c 100644
--- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
+++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
@@ -50,6 +50,19 @@ NavigateToPoseNavigator::configure(
     "goal_pose",
     rclcpp::SystemDefaultsQoS(),
     std::bind(&NavigateToPoseNavigator::onGoalPoseReceived, this, std::placeholders::_1));
+
+  if (!node->has_parameter(getName() + ".enable_groot_monitoring")) {
+    node->declare_parameter(getName() + ".enable_groot_monitoring", false);
+  }
+
+  if (!node->has_parameter(getName() + ".groot_server_port")) {
+    node->declare_parameter(getName() + ".groot_server_port", 1667);
+  }
+
+  bt_action_server_->setGrootMonitoring(
+    node->get_parameter(getName() + ".enable_groot_monitoring").as_bool(),
+    node->get_parameter(getName() + ".groot_server_port").as_int());
+
   return true;
 }
 
@@ -88,7 +101,8 @@ NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
   auto bt_xml_filename = goal->behavior_tree;
 
   if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) {
-    bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+    bt_action_server_->setInternalError(
+      ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
       std::string("Error loading XML file: ") + bt_xml_filename + ". Navigation canceled.");
     return false;
   }
@@ -103,13 +117,15 @@ NavigateToPoseNavigator::goalCompleted(
 {
   if (result->error_code == 0) {
     if (bt_action_server_->populateInternalError(result)) {
-      RCLCPP_WARN(logger_,
+      RCLCPP_WARN(
+        logger_,
         "NavigateToPoseNavigator::goalCompleted, internal error %d:%s.",
         result->error_code,
         result->error_msg.c_str());
     }
   } else {
-    RCLCPP_WARN(logger_, "NavigateToPoseNavigator::goalCompleted error %d:%s.",
+    RCLCPP_WARN(
+      logger_, "NavigateToPoseNavigator::goalCompleted error %d:%s.",
       result->error_code,
       result->error_msg.c_str());
   }
@@ -225,7 +241,8 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
       feedback_utils_.global_frame, feedback_utils_.robot_frame,
       feedback_utils_.transform_tolerance))
   {
-    bt_action_server_->setInternalError(ActionT::Result::TF_ERROR,
+    bt_action_server_->setInternalError(
+      ActionT::Result::TF_ERROR,
       "Initial robot pose is not available.");
     return false;
   }
@@ -235,7 +252,8 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
       goal->pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame,
       feedback_utils_.transform_tolerance))
   {
-    bt_action_server_->setInternalError(ActionT::Result::TF_ERROR,
+    bt_action_server_->setInternalError(
+      ActionT::Result::TF_ERROR,
       "Failed to transform a goal pose provided with frame_id '" +
       goal->pose.header.frame_id +
       "' to the global frame '" +
diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md
index eca25192b6e..a34976cce23 100644
--- a/nav2_collision_monitor/README.md
+++ b/nav2_collision_monitor/README.md
@@ -36,7 +36,7 @@ The zones around the robot can take the following shapes:
 * Arbitrary user-defined polygon relative to the robot base frame, which can be static in a configuration file or dynamically changing via a topic interface.
 * Robot footprint polygon, which is used in the approach behavior model only. Will use the static user-defined polygon or the footprint topic to allow it to be dynamically adjusted over time.
 * Circle: is made for the best performance and could be used in the cases where the zone or robot footprint could be approximated by round shape.
-* VelocityPolygon: allow switching of polygons based on the command velocity. When the velocity is covered by multiple sub polygons, the first sub polygon in the `velocity_polygons` list will be used. This is useful for robots to set different safety zones based on their velocity (e.g. a robot that has a larger safety zone when moving at 1.0 m/s than when moving at 0.5 m/s). 
+* VelocityPolygon: allow switching of polygons based on the command velocity. When the velocity is covered by multiple sub polygons, the first sub polygon in the `velocity_polygons` list will be used. This is useful for robots to set different safety zones based on their velocity (e.g. a robot that has a larger safety zone when moving at 1.0 m/s than when moving at 0.5 m/s).
 
 
 The data may be obtained from different data sources:
diff --git a/nav2_collision_monitor/launch/collision_detector_node.launch.py b/nav2_collision_monitor/launch/collision_detector_node.launch.py
index 548aca4e2bb..43714ab3607 100644
--- a/nav2_collision_monitor/launch/collision_detector_node.launch.py
+++ b/nav2_collision_monitor/launch/collision_detector_node.launch.py
@@ -17,20 +17,16 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, GroupAction
 from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration, PythonExpression
-from launch.substitutions import NotEqualsSubstitution
-from launch_ros.actions import LoadComposableNodes, SetParameter
-from launch_ros.actions import Node
-from launch_ros.actions import PushROSNamespace
+from launch.substitutions import LaunchConfiguration, NotEqualsSubstitution, PythonExpression
+from launch_ros.actions import LoadComposableNodes, Node, PushROSNamespace, SetParameter
 from launch_ros.descriptions import ComposableNode
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     # Environment
     package_dir = get_package_share_directory('nav2_collision_monitor')
 
diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py
index 3dd9d01a0d4..5bad8f7b7d3 100644
--- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py
+++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py
@@ -17,20 +17,16 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, GroupAction
 from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration, PythonExpression
-from launch.substitutions import NotEqualsSubstitution
-from launch_ros.actions import LoadComposableNodes, SetParameter
-from launch_ros.actions import Node
-from launch_ros.actions import PushROSNamespace
+from launch.substitutions import LaunchConfiguration, NotEqualsSubstitution, PythonExpression
+from launch_ros.actions import LoadComposableNodes, Node, PushROSNamespace, SetParameter
 from launch_ros.descriptions import ComposableNode, ParameterFile
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     # Environment
     package_dir = get_package_share_directory('nav2_collision_monitor')
 
diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml
index d6b3cd25fda..eac7486913c 100644
--- a/nav2_collision_monitor/params/collision_monitor_params.yaml
+++ b/nav2_collision_monitor/params/collision_monitor_params.yaml
@@ -81,7 +81,7 @@ collision_monitor:
         theta_max: 1.0
       # This is the last polygon to be checked, it should cover the entire range of robot's velocities
       # It is used as the stopped polygon when the robot is not moving and as a fallback if the velocity
-      # is not covered by any of the other sub-polygons 
+      # is not covered by any of the other sub-polygons
       stopped:
         points: "[[0.25, 0.25], [0.25, -0.25], [-0.25, -0.25], [-0.25, 0.25]]"
         linear_min: -1.0
diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
index c336c59782c..472a7d255e3 100644
--- a/nav2_collision_monitor/src/collision_monitor_node.cpp
+++ b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -188,7 +188,7 @@ CollisionMonitor::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
 void CollisionMonitor::cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped::SharedPtr msg)
 {
   // If message contains NaN or Inf, ignore
-  if (!nav2_util::validateTwist(*msg)) {
+  if (!nav2_util::validateTwist(msg->twist)) {
     RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!");
     return;
   }
diff --git a/nav2_collision_monitor/test/kinematics_test.cpp b/nav2_collision_monitor/test/kinematics_test.cpp
index 9fc183f86ba..5b373ac93af 100644
--- a/nav2_collision_monitor/test/kinematics_test.cpp
+++ b/nav2_collision_monitor/test/kinematics_test.cpp
@@ -89,7 +89,7 @@ TEST(KinematicsTest, testProjectState)
   EXPECT_NEAR(vel.tw, M_PI / 4.0, EPSILON);  // should be the same
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_common/cmake/nav2_package.cmake b/nav2_common/cmake/nav2_package.cmake
index 3f4977193c8..5bf034baf9e 100644
--- a/nav2_common/cmake/nav2_package.cmake
+++ b/nav2_common/cmake/nav2_package.cmake
@@ -29,10 +29,10 @@ macro(nav2_package)
 
   # Default to C++17
   if(NOT CMAKE_CXX_STANDARD)
-    if ("cxx_std_17" IN_LIST CMAKE_CXX_COMPILE_FEATURES)
+    if("cxx_std_17" IN_LIST CMAKE_CXX_COMPILE_FEATURES)
       set(CMAKE_CXX_STANDARD 17)
     else()
-      message( FATAL_ERROR "cxx_std_17 could not be found.")
+      message(FATAL_ERROR "cxx_std_17 could not be found.")
     endif()
   endif()
 
diff --git a/nav2_common/nav2_common/launch/has_node_params.py b/nav2_common/nav2_common/launch/has_node_params.py
index 41f005bbc50..fa36f280785 100644
--- a/nav2_common/nav2_common/launch/has_node_params.py
+++ b/nav2_common/nav2_common/launch/has_node_params.py
@@ -12,14 +12,11 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from typing import List
-from typing import Text
-
 import launch
 import yaml
 
 
-class HasNodeParams(launch.Substitution):
+class HasNodeParams(launch.Substitution):  # type: ignore[misc]
     """
     Substitution that checks if a param file contains parameters for a node.
 
@@ -27,7 +24,7 @@ class HasNodeParams(launch.Substitution):
     """
 
     def __init__(
-        self, source_file: launch.SomeSubstitutionsType, node_name: Text
+        self, source_file: launch.SomeSubstitutionsType, node_name: str
     ) -> None:
         super().__init__()
         """
@@ -37,25 +34,25 @@ def __init__(
     :param: node_name the name of the node to check
     """
 
-        from launch.utilities import (
-            normalize_to_list_of_substitutions,
-        )  # import here to avoid loop
+        # import here to avoid loop
+        from launch.utilities import normalize_to_list_of_substitutions
 
-        self.__source_file = normalize_to_list_of_substitutions(source_file)
+        self.__source_file: list[launch.Substitution] = \
+            normalize_to_list_of_substitutions(source_file)
         self.__node_name = node_name
 
     @property
-    def name(self) -> List[launch.Substitution]:
+    def name(self) -> list[launch.Substitution]:
         """Getter for name."""
         return self.__source_file
 
-    def describe(self) -> Text:
+    def describe(self) -> str:
         """Return a description of this substitution as a string."""
         return ''
 
-    def perform(self, context: launch.LaunchContext) -> Text:
+    def perform(self, context: launch.LaunchContext) -> str:
         yaml_filename = launch.utilities.perform_substitutions(context, self.name)
-        data = yaml.safe_load(open(yaml_filename, 'r'))
+        data = yaml.safe_load(open(yaml_filename))
 
         if self.__node_name in data.keys():
             return 'True'
diff --git a/nav2_common/nav2_common/launch/replace_string.py b/nav2_common/nav2_common/launch/replace_string.py
index 076ac16f21f..e379c045457 100644
--- a/nav2_common/nav2_common/launch/replace_string.py
+++ b/nav2_common/nav2_common/launch/replace_string.py
@@ -13,12 +13,12 @@
 # limitations under the License.
 
 import tempfile
-from typing import Dict, List, Optional, Text
+from typing import Optional
 
 import launch
 
 
-class ReplaceString(launch.Substitution):
+class ReplaceString(launch.Substitution):  # type: ignore[misc]
     """
     Substitution that replaces strings on a given file.
 
@@ -28,16 +28,17 @@ class ReplaceString(launch.Substitution):
     def __init__(
         self,
         source_file: launch.SomeSubstitutionsType,
-        replacements: Dict,
+        replacements: dict[str, launch.SomeSubstitutionsType],
         condition: Optional[launch.Condition] = None,
     ) -> None:
         super().__init__()
 
-        from launch.utilities import (
-            normalize_to_list_of_substitutions,
-        )  # import here to avoid loop
+        from launch.utilities import normalize_to_list_of_substitutions
 
-        self.__source_file = normalize_to_list_of_substitutions(source_file)
+        # import here to avoid loop
+
+        self.__source_file: list[launch.Substitution] = \
+            normalize_to_list_of_substitutions(source_file)
         self.__replacements = {}
         for key in replacements:
             self.__replacements[key] = normalize_to_list_of_substitutions(
@@ -46,7 +47,7 @@ def __init__(
         self.__condition = condition
 
     @property
-    def name(self) -> List[launch.Substitution]:
+    def name(self) -> list[launch.Substitution]:
         """Getter for name."""
         return self.__source_file
 
@@ -55,17 +56,19 @@ def condition(self) -> Optional[launch.Condition]:
         """Getter for condition."""
         return self.__condition
 
-    def describe(self) -> Text:
+    def describe(self) -> str:
         """Return a description of this substitution as a string."""
         return ''
 
-    def perform(self, context: launch.LaunchContext) -> Text:
-        yaml_filename = launch.utilities.perform_substitutions(context, self.name)
+    def perform(self, context: launch.LaunchContext) -> str:
+        yaml_filename: str = launch.utilities.perform_substitutions(
+            context, self.name
+        )
         if self.__condition is None or self.__condition.evaluate(context):
             output_file = tempfile.NamedTemporaryFile(mode='w', delete=False)
             replacements = self.resolve_replacements(context)
             try:
-                input_file = open(yaml_filename, 'r')
+                input_file = open(yaml_filename)
                 self.replace(input_file, output_file, replacements)
             except Exception as err:  # noqa: B902
                 print('ReplaceString substitution error: ', err)
@@ -76,7 +79,7 @@ def perform(self, context: launch.LaunchContext) -> Text:
         else:
             return yaml_filename
 
-    def resolve_replacements(self, context):
+    def resolve_replacements(self, context: launch.LaunchContext) -> dict[str, str]:
         resolved_replacements = {}
         for key in self.__replacements:
             resolved_replacements[key] = launch.utilities.perform_substitutions(
@@ -84,7 +87,8 @@ def resolve_replacements(self, context):
             )
         return resolved_replacements
 
-    def replace(self, input_file, output_file, replacements):
+    def replace(self, input_file: launch.SomeSubstitutionsType,
+                output_file: launch.SomeSubstitutionsType, replacements: dict[str, str]) -> None:
         for line in input_file:
             for key, value in replacements.items():
                 if isinstance(key, str) and isinstance(value, str):
diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py
index 11aa77227e3..49ec39b5377 100644
--- a/nav2_common/nav2_common/launch/rewritten_yaml.py
+++ b/nav2_common/nav2_common/launch/rewritten_yaml.py
@@ -12,27 +12,30 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from collections.abc import Generator
 import tempfile
-from typing import Dict, List, Optional, Text
+from typing import Optional, TypeAlias, Union
 
 import launch
 import yaml
 
+YamlValue: TypeAlias = Union[str, int, float, bool]
+
 
 class DictItemReference:
 
-    def __init__(self, dictionary, key):
+    def __init__(self, dictionary: dict[str, YamlValue], key: str):
         self.dictionary = dictionary
         self.dictKey = key
 
-    def key(self):
+    def key(self) -> str:
         return self.dictKey
 
-    def setValue(self, value):
+    def setValue(self, value: YamlValue) -> None:
         self.dictionary[self.dictKey] = value
 
 
-class RewrittenYaml(launch.Substitution):
+class RewrittenYaml(launch.Substitution):  # type: ignore[misc]
     """
     Substitution that modifies the given YAML file.
 
@@ -42,10 +45,10 @@ class RewrittenYaml(launch.Substitution):
     def __init__(
         self,
         source_file: launch.SomeSubstitutionsType,
-        param_rewrites: Dict,
+        param_rewrites: dict[str, launch.SomeSubstitutionsType],
         root_key: Optional[launch.SomeSubstitutionsType] = None,
-        key_rewrites: Optional[Dict] = None,
-        convert_types=False,
+        key_rewrites: Optional[dict[str, launch.SomeSubstitutionsType]] = None,
+        convert_types: bool = False,
     ) -> None:
         super().__init__()
         """
@@ -58,11 +61,11 @@ def __init__(
         :param: convert_types whether to attempt converting the string to a number or boolean
         """
 
-        from launch.utilities import (
-            normalize_to_list_of_substitutions,
-        )  # import here to avoid loop
+        # import here to avoid loop
+        from launch.utilities import normalize_to_list_of_substitutions
 
-        self.__source_file = normalize_to_list_of_substitutions(source_file)
+        self.__source_file: list[launch.Substitution] = \
+            normalize_to_list_of_substitutions(source_file)
         self.__param_rewrites = {}
         self.__key_rewrites = {}
         self.__convert_types = convert_types
@@ -80,19 +83,19 @@ def __init__(
             self.__root_key = normalize_to_list_of_substitutions(root_key)
 
     @property
-    def name(self) -> List[launch.Substitution]:
+    def name(self) -> list[launch.Substitution]:
         """Getter for name."""
         return self.__source_file
 
-    def describe(self) -> Text:
+    def describe(self) -> str:
         """Return a description of this substitution as a string."""
         return ''
 
-    def perform(self, context: launch.LaunchContext) -> Text:
+    def perform(self, context: launch.LaunchContext) -> str:
         yaml_filename = launch.utilities.perform_substitutions(context, self.name)
         rewritten_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False)
         param_rewrites, keys_rewrites = self.resolve_rewrites(context)
-        data = yaml.safe_load(open(yaml_filename, 'r'))
+        data = yaml.safe_load(open(yaml_filename))
         self.substitute_params(data, param_rewrites)
         self.add_params(data, param_rewrites)
         self.substitute_keys(data, keys_rewrites)
@@ -104,7 +107,8 @@ def perform(self, context: launch.LaunchContext) -> Text:
         rewritten_yaml.close()
         return rewritten_yaml.name
 
-    def resolve_rewrites(self, context):
+    def resolve_rewrites(self, context: launch.LaunchContext) -> \
+            tuple[dict[str, str], dict[str, str]]:
         resolved_params = {}
         for key in self.__param_rewrites:
             resolved_params[key] = launch.utilities.perform_substitutions(
@@ -117,7 +121,8 @@ def resolve_rewrites(self, context):
             )
         return resolved_params, resolved_keys
 
-    def substitute_params(self, yaml, param_rewrites):
+    def substitute_params(self, yaml: dict[str, YamlValue],
+                          param_rewrites: dict[str, str]) -> None:
         # substitute leaf-only parameters
         for key in self.getYamlLeafKeys(yaml):
             if key.key() in param_rewrites:
@@ -133,7 +138,8 @@ def substitute_params(self, yaml, param_rewrites):
                 yaml_keys = path.split('.')
                 yaml = self.updateYamlPathVals(yaml, yaml_keys, rewrite_val)
 
-    def add_params(self, yaml, param_rewrites):
+    def add_params(self, yaml: dict[str, YamlValue],
+                   param_rewrites: dict[str, str]) -> None:
         # add new total path parameters
         yaml_paths = self.pathify(yaml)
         for path in param_rewrites:
@@ -143,7 +149,10 @@ def add_params(self, yaml, param_rewrites):
                 if 'ros__parameters' in yaml_keys:
                     yaml = self.updateYamlPathVals(yaml, yaml_keys, new_val)
 
-    def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val):
+    def updateYamlPathVals(
+            self, yaml: dict[str, YamlValue],
+            yaml_key_list: list[str], rewrite_val: YamlValue) -> dict[str, YamlValue]:
+
         for key in yaml_key_list:
             if key == yaml_key_list[-1]:
                 yaml[key] = rewrite_val
@@ -154,12 +163,15 @@ def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val):
                     yaml[int(key)], yaml_key_list, rewrite_val
                 )
             else:
-                yaml[key] = self.updateYamlPathVals(
-                    yaml.get(key, {}), yaml_key_list, rewrite_val
+                yaml[key] = self.updateYamlPathVals(  # type: ignore[assignment]
+                    yaml.get(key, {}),  # type: ignore[arg-type]
+                    yaml_key_list,
+                    rewrite_val
                 )
         return yaml
 
-    def substitute_keys(self, yaml, key_rewrites):
+    def substitute_keys(
+            self, yaml: dict[str, YamlValue], key_rewrites: dict[str, str]) -> None:
         if len(key_rewrites) != 0:
             for key in list(yaml.keys()):
                 val = yaml[key]
@@ -170,20 +182,31 @@ def substitute_keys(self, yaml, key_rewrites):
                 if isinstance(val, dict):
                     self.substitute_keys(val, key_rewrites)
 
-    def getYamlLeafKeys(self, yamlData):
-        try:
-            for key in yamlData.keys():
-                for k in self.getYamlLeafKeys(yamlData[key]):
-                    yield k
-                yield DictItemReference(yamlData, key)
-        except AttributeError:
+    def getYamlLeafKeys(self, yamlData: dict[str, YamlValue]) -> \
+            Generator[DictItemReference, None, None]:
+        if not isinstance(yamlData, dict):
             return
 
-    def pathify(self, d, p=None, paths=None, joinchar='.'):
+        for key in yamlData.keys():
+            child = yamlData[key]
+
+            if isinstance(child, dict):
+                # Recursively process nested dictionaries
+                yield from self.getYamlLeafKeys(child)
+
+            yield DictItemReference(yamlData, key)
+
+    def pathify(
+            self, d: Union[dict[str, YamlValue], list[YamlValue], YamlValue],
+            p: Optional[str] = None,
+            paths: Optional[dict[str, YamlValue]] = None,
+            joinchar: str = '.') -> dict[str, YamlValue]:
         if p is None:
             paths = {}
             self.pathify(d, '', paths, joinchar=joinchar)
             return paths
+
+        assert paths is not None
         pn = p
         if p != '':
             pn += joinchar
@@ -196,8 +219,9 @@ def pathify(self, d, p=None, paths=None, joinchar='.'):
                 self.pathify(e, pn + str(idx), paths, joinchar=joinchar)
         else:
             paths[p] = d
+        return paths
 
-    def convert(self, text_value):
+    def convert(self, text_value: str) -> YamlValue:
         if self.__convert_types:
             # try converting to int or float
             try:
diff --git a/nav2_common/package.xml b/nav2_common/package.xml
index 1584fafa646..f2cebe1c8f7 100644
--- a/nav2_common/package.xml
+++ b/nav2_common/package.xml
@@ -12,6 +12,7 @@
   <depend>osrf_pycommon</depend>
   <depend>rclpy</depend>
   <depend>python3-yaml</depend>
+  <depend>python3-types-pyyaml</depend>
 
   <buildtool_depend>ament_cmake_core</buildtool_depend>
 
diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp
index c4bb7e52740..19ab65bd325 100644
--- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp
+++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp
@@ -404,7 +404,7 @@ class SmootherTest : public ::testing::Test
   int cusp_i_ = -1;
   QualityCriterion3 mvmt_smoothness_criterion_ =
     [this](int i, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p,
-    const Eigen::Vector3d & next_p) {
+      const Eigen::Vector3d & next_p) {
       Eigen::Vector2d prev_mvmt = p.block<2, 1>(0, 0) - prev_p.block<2, 1>(0, 0);
       Eigen::Vector2d next_mvmt = next_p.block<2, 1>(0, 0) - p.block<2, 1>(0, 0);
       if (i == cusp_i_) {
@@ -986,7 +986,7 @@ TEST_F(SmootherTest, testingDownsamplingUpsampling)
   int cusp_i_out = 6;  // for upsampled path
   QualityCriterion3 mvmt_smoothness_criterion_out =
     [&cusp_i_out](int i, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p,
-    const Eigen::Vector3d & next_p) {
+      const Eigen::Vector3d & next_p) {
       Eigen::Vector2d prev_mvmt = p.block<2, 1>(0, 0) - prev_p.block<2, 1>(0, 0);
       Eigen::Vector2d next_mvmt = next_p.block<2, 1>(0, 0) - p.block<2, 1>(0, 0);
       if (i == cusp_i_out) {
diff --git a/nav2_controller/README.md b/nav2_controller/README.md
index c718a4e56f7..d2b468b1b4d 100644
--- a/nav2_controller/README.md
+++ b/nav2_controller/README.md
@@ -4,7 +4,7 @@ The Nav2 Controller is a Task Server in Nav2 that implements the `nav2_msgs::act
 
 An execution module implementing the `nav2_msgs::action::FollowPath` action server is responsible for generating command velocities for the robot, given the computed path from the planner module in `nav2_planner`. The nav2_controller package is designed to be loaded with multiple plugins for path execution. The plugins need to implement functions in the virtual base class defined in the `controller` header file in `nav2_core` package. It also contains progress checkers and goal checker plugins to abstract out that logic from specific controller implementations.
 
-See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available controller plugins. 
+See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available controller plugins.
 
 See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-controller-server.html) for additional parameter descriptions and a [tutorial about writing controller plugins](https://docs.nav2.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html).
 
diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp
index eeb02a554cb..06fd66ff364 100644
--- a/nav2_controller/src/controller_server.cpp
+++ b/nav2_controller/src/controller_server.cpp
@@ -675,8 +675,8 @@ void ControllerServer::computeAndPublishVelocity()
   geometry_msgs::msg::PoseStamped robot_pose_in_path_frame;
   rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance()));
   if (!nav_2d_utils::transformPose(
-          costmap_ros_->getTfBuffer(), current_path_.header.frame_id, pose,
-          robot_pose_in_path_frame, tolerance))
+      costmap_ros_->getTfBuffer(), current_path_.header.frame_id, pose,
+      robot_pose_in_path_frame, tolerance))
   {
     throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame");
   }
@@ -691,7 +691,8 @@ void ControllerServer::computeAndPublishVelocity()
       double curr_min_dist = std::numeric_limits<double>::max();
       for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) {
         double curr_dist =
-          nav2_util::geometry_utils::euclidean_distance(robot_pose_in_path_frame,
+          nav2_util::geometry_utils::euclidean_distance(
+          robot_pose_in_path_frame,
           current_path.poses[curr_idx]);
         if (curr_dist < curr_min_dist) {
           curr_min_dist = curr_dist;
@@ -702,8 +703,9 @@ void ControllerServer::computeAndPublishVelocity()
     };
 
   const std::size_t closest_pose_idx = find_closest_pose_idx();
-  feedback->distance_to_goal = nav2_util::geometry_utils::calculate_path_length(current_path_,
-      closest_pose_idx);
+  feedback->distance_to_goal = nav2_util::geometry_utils::calculate_path_length(
+    current_path_,
+    closest_pose_idx);
   action_server_->publish_feedback(feedback);
 }
 
@@ -758,6 +760,10 @@ void ControllerServer::updateGlobalPath()
 void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity)
 {
   auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>(velocity);
+  if (!nav2_util::validateTwist(cmd_vel->twist)) {
+    RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!");
+    return;
+  }
   if (vel_publisher_->is_activated() && vel_publisher_->get_subscription_count() > 0) {
     vel_publisher_->publish(std::move(cmd_vel));
   }
diff --git a/nav2_controller/test/test_dynamic_parameters.cpp b/nav2_controller/test/test_dynamic_parameters.cpp
index 6fcf574c3f1..b8f03e19991 100644
--- a/nav2_controller/test/test_dynamic_parameters.cpp
+++ b/nav2_controller/test/test_dynamic_parameters.cpp
@@ -79,7 +79,7 @@ TEST(WPTest, test_dynamic_parameters)
   EXPECT_EQ(controller->get_parameter("failure_tolerance").as_double(), 5.0);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_costmap_2d/README.md b/nav2_costmap_2d/README.md
index a794f6430ff..0677cedb23c 100644
--- a/nav2_costmap_2d/README.md
+++ b/nav2_costmap_2d/README.md
@@ -1,10 +1,10 @@
 # Nav2 Costmap_2d
 
-The costmap_2d package is responsible for building a 2D costmap of the environment, consisting of several "layers" of data about the environment. It can be initialized via the map server or a local rolling window and updates the layers by taking observations from sensors. A plugin interface allows for the layers to be combined into the costmap and finally inflated via an inflation radius based on the robot footprint. The nav2 version of the costmap_2d package is mostly a direct ROS2 port of the ROS1 navigation stack version, with minimal notable changes necessary due to support in ROS2. 
+The costmap_2d package is responsible for building a 2D costmap of the environment, consisting of several "layers" of data about the environment. It can be initialized via the map server or a local rolling window and updates the layers by taking observations from sensors. A plugin interface allows for the layers to be combined into the costmap and finally inflated via an inflation radius based on the robot footprint. The nav2 version of the costmap_2d package is mostly a direct ROS2 port of the ROS1 navigation stack version, with minimal notable changes necessary due to support in ROS2.
 
 See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-costmaps.html) for additional parameter descriptions for the costmap and its included plugins. The [tutorials](https://docs.nav2.org/tutorials/index.html) and [first-time setup guides](https://docs.nav2.org/setup_guides/index.html) also provide helpful context for working with the costmap 2D package and its layers. A [tutorial](https://docs.nav2.org/plugin_tutorials/docs/writing_new_costmap2d_plugin.html) is also provided to explain how to create costmap plugins.
 
-See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. 
+See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins.
 
 ## To visualize the voxels in RVIZ:
 - Make sure `publish_voxel_map` in `voxel_layer` param's scope is set to `True`.
diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml
index 16e2f4ba61a..6d2bde4a1c3 100644
--- a/nav2_costmap_2d/costmap_plugins.xml
+++ b/nav2_costmap_2d/costmap_plugins.xml
@@ -35,4 +35,3 @@
     </class>
   </library>
 </class_libraries>
-
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp
index 844902dd100..eae9a90b524 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp
@@ -25,6 +25,7 @@
 #include "nav2_msgs/srv/clear_entire_costmap.hpp"
 #include "nav2_costmap_2d/costmap_layer.hpp"
 #include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/service_server.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -41,7 +42,8 @@ class ClearCostmapService
   /**
    * @brief A constructor
    */
-  ClearCostmapService(const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap);
+  ClearCostmapService(
+    const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap);
 
   /**
    * @brief A constructor
@@ -74,7 +76,8 @@ class ClearCostmapService
   unsigned char reset_value_;
 
   // Server for clearing the costmap
-  rclcpp::Service<nav2_msgs::srv::ClearCostmapExceptRegion>::SharedPtr clear_except_service_;
+  nav2_util::ServiceServer<nav2_msgs::srv::ClearCostmapExceptRegion,
+    std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>::SharedPtr clear_except_service_;
   /**
    * @brief Callback to clear costmap except in a given region
    */
@@ -83,7 +86,8 @@ class ClearCostmapService
     const std::shared_ptr<nav2_msgs::srv::ClearCostmapExceptRegion::Request> request,
     const std::shared_ptr<nav2_msgs::srv::ClearCostmapExceptRegion::Response> response);
 
-  rclcpp::Service<nav2_msgs::srv::ClearCostmapAroundRobot>::SharedPtr clear_around_service_;
+  nav2_util::ServiceServer<nav2_msgs::srv::ClearCostmapAroundRobot,
+    std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>::SharedPtr clear_around_service_;
   /**
    * @brief Callback to clear costmap in a given region
    */
@@ -92,7 +96,8 @@ class ClearCostmapService
     const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Request> request,
     const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Response> response);
 
-  rclcpp::Service<nav2_msgs::srv::ClearEntireCostmap>::SharedPtr clear_entire_service_;
+  nav2_util::ServiceServer<nav2_msgs::srv::ClearEntireCostmap,
+    std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>::SharedPtr clear_entire_service_;
   /**
    * @brief Callback to clear costmap
    */
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp
index b30179977d5..fbacf15aa24 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp
@@ -173,6 +173,15 @@ class Costmap2D
    */
   void mapToWorld(unsigned int mx, unsigned int my, double & wx, double & wy) const;
 
+  /**
+  * @brief  Convert from map coordinates to world coordinates with no bounds checking
+  * @param  wx The x world coordinate
+  * @param  wy The y world coordinate
+  * @param  mx Will be set to the associated map x coordinate
+  * @param  my Will be set to the associated map y coordinate
+  */
+  void mapToWorldNoBounds(int mx, int my, double & wx, double & wy) const;
+
   /**
    * @brief  Convert from world coordinates to map coordinates
    * @param  wx The x world coordinate
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp
index 41041c44b6e..d6e1b8038b3 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp
@@ -54,6 +54,7 @@
 #include "nav2_util/lifecycle_node.hpp"
 #include "tf2/LinearMath/Quaternion.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_util/service_server.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -180,7 +181,9 @@ class Costmap2DPublisher
     costmap_raw_update_pub_;
 
   // Service for getting the costmaps
-  rclcpp::Service<nav2_msgs::srv::GetCostmap>::SharedPtr costmap_service_;
+  nav2_util::ServiceServer<nav2_msgs::srv::GetCostmap,
+    std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>::SharedPtr
+    costmap_service_;
 
   float grid_resolution_;
   unsigned int grid_width_, grid_height_;
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
index 5fa4c1cacc2..b7f3bf80c83 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
@@ -60,6 +60,7 @@
 #include "tf2_ros/transform_listener.h"
 #include "tf2/time.hpp"
 #include "tf2/transform_datatypes.hpp"
+#include "nav2_util/service_server.hpp"
 
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
@@ -414,7 +415,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   std::vector<geometry_msgs::msg::Point> padded_footprint_;
 
   // Services
-  rclcpp::Service<nav2_msgs::srv::GetCosts>::SharedPtr get_cost_service_;
+  nav2_util::ServiceServer<nav2_msgs::srv::GetCosts,
+    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr get_cost_service_;
   std::unique_ptr<ClearCostmapService> clear_costmap_service_;
 
   // Dynamic parameters handler
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
index d1128b5ccc6..00a3cb833c1 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
@@ -49,6 +49,7 @@
 #include "std_srvs/srv/set_bool.hpp"
 #include "nav2_costmap_2d/layer.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav2_util/service_server.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -241,7 +242,8 @@ class CostmapFilter : public Layer
   /**
    * @brief: A service to enable/disable costmap filter
    */
-  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr enable_service_;
+  nav2_util::ServiceServer<std_srvs::srv::SetBool,
+    std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>::SharedPtr enable_service_;
 
 private:
   /**
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp
index 4f645422fdd..a761415c5e4 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp
@@ -48,6 +48,15 @@ class CostmapTopicCollisionChecker
     FootprintSubscriber & footprint_sub,
     std::string name = "collision_checker");
 
+  /**
+   * @brief Alternative constructor with a footprint string instead of a subscriber. It needs to be
+   * defined relative to the robot frame
+   */
+  CostmapTopicCollisionChecker(
+    CostmapSubscriber & costmap_sub,
+    std::string footprint_string,
+    std::string name = "collision_checker");
+
   /**
    * @brief A destructor
    */
@@ -90,10 +99,11 @@ class CostmapTopicCollisionChecker
   // Name used for logging
   std::string name_;
   CostmapSubscriber & costmap_sub_;
-  FootprintSubscriber & footprint_sub_;
+  FootprintSubscriber * footprint_sub_ = nullptr;
   FootprintCollisionChecker<std::shared_ptr<Costmap2D>> collision_checker_;
   rclcpp::Clock::SharedPtr clock_;
   Footprint footprint_;
+  std::string footprint_string_;
 };
 
 }  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
index f05726a6b32..dcf1a4c39c3 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
@@ -83,11 +83,13 @@ void CostmapFilter::onInitialize()
     transform_tolerance_ = tf2::durationFromSec(transform_tolerance);
 
     // Costmap Filter enabling service
-    enable_service_ = node->create_service<std_srvs::srv::SetBool>(
+    enable_service_ = std::make_shared<nav2_util::ServiceServer<std_srvs::srv::SetBool,
+        std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>>(
       name_ + "/toggle_filter",
+      node,
       std::bind(
-        &CostmapFilter::enableCallback, this,
-        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+        &CostmapFilter::enableCallback, this, std::placeholders::_1,
+        std::placeholders::_2, std::placeholders::_3));
   } catch (const std::exception & ex) {
     RCLCPP_ERROR(logger_, "Parameter problem: %s", ex.what());
     throw ex;
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
index a05548a8cde..7a040d2e83f 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -240,7 +240,7 @@ void ObstacleLayer::onInitialize()
       #else
       sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
           rclcpp_lifecycle::LifecycleNode>>(
-          node, topic, custom_qos_profile.get_rmw_qos_profile(), sub_opt);
+        node, topic, custom_qos_profile.get_rmw_qos_profile(), sub_opt);
       #endif
 
       sub->unsubscribe();
@@ -280,7 +280,7 @@ void ObstacleLayer::onInitialize()
       #else
       sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2,
           rclcpp_lifecycle::LifecycleNode>>(
-          node, topic, custom_qos_profile.get_rmw_qos_profile(), sub_opt);
+        node, topic, custom_qos_profile.get_rmw_qos_profile(), sub_opt);
       #endif
 
       sub->unsubscribe();
diff --git a/nav2_costmap_2d/plugins/plugin_container_layer.cpp b/nav2_costmap_2d/plugins/plugin_container_layer.cpp
index 7d171f6b6a1..743a4751e1a 100644
--- a/nav2_costmap_2d/plugins/plugin_container_layer.cpp
+++ b/nav2_costmap_2d/plugins/plugin_container_layer.cpp
@@ -35,12 +35,15 @@ void PluginContainerLayer::onInitialize()
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  nav2_util::declare_parameter_if_not_declared(node, name_ + "." + "enabled",
-      rclcpp::ParameterValue(true));
-  nav2_util::declare_parameter_if_not_declared(node, name_ + "." + "plugins",
-      rclcpp::ParameterValue(std::vector<std::string>{}));
-  nav2_util::declare_parameter_if_not_declared(node, name_ + "." + "combination_method",
-      rclcpp::ParameterValue(1));
+  nav2_util::declare_parameter_if_not_declared(
+    node, name_ + "." + "enabled",
+    rclcpp::ParameterValue(true));
+  nav2_util::declare_parameter_if_not_declared(
+    node, name_ + "." + "plugins",
+    rclcpp::ParameterValue(std::vector<std::string>{}));
+  nav2_util::declare_parameter_if_not_declared(
+    node, name_ + "." + "combination_method",
+    rclcpp::ParameterValue(1));
 
   node->get_parameter(name_ + "." + "enabled", enabled_);
   node->get_parameter(name_ + "." + "plugins", plugin_names_);
@@ -185,7 +188,7 @@ bool PluginContainerLayer::isClearable()
   for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
     ++plugin)
   {
-    if((*plugin)->isClearable()) {
+    if ((*plugin)->isClearable()) {
       return true;
     }
   }
diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp
index 9f0a69f80ed..4ef50c1f0d4 100644
--- a/nav2_costmap_2d/plugins/static_layer.cpp
+++ b/nav2_costmap_2d/plugins/static_layer.cpp
@@ -512,8 +512,9 @@ StaticLayer::dynamicParametersCallback(
         if (footprint_clearing_enabled_) {
           restore_cleared_footprint_ = parameter.as_bool();
         } else {
-          RCLCPP_WARN(logger_, "restore_cleared_footprint cannot be used "
-                      "when footprint_clearing_enabled is False. Rejecting parameter update.");
+          RCLCPP_WARN(
+            logger_, "restore_cleared_footprint cannot be used "
+            "when footprint_clearing_enabled is False. Rejecting parameter update.");
         }
       }
     }
diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp
index e11aca2ce5a..b27b6841ddc 100644
--- a/nav2_costmap_2d/src/clear_costmap_service.cpp
+++ b/nav2_costmap_2d/src/clear_costmap_service.cpp
@@ -40,20 +40,26 @@ ClearCostmapService::ClearCostmapService(
   logger_ = node->get_logger();
   reset_value_ = costmap_.getCostmap()->getDefaultValue();
 
-  clear_except_service_ = node->create_service<ClearExceptRegion>(
-    "clear_except_" + costmap_.getName(),
+  clear_except_service_ = std::make_shared<nav2_util::ServiceServer<ClearExceptRegion,
+      std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>>(
+    std::string("clear_except_") + costmap_.getName(),
+    node,
     std::bind(
       &ClearCostmapService::clearExceptRegionCallback, this,
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
-  clear_around_service_ = node->create_service<ClearAroundRobot>(
-    "clear_around_" + costmap.getName(),
+  clear_around_service_ = std::make_shared<nav2_util::ServiceServer<ClearAroundRobot,
+      std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>>(
+    std::string("clear_around_") + costmap_.getName(),
+    node,
     std::bind(
       &ClearCostmapService::clearAroundRobotCallback, this,
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
-  clear_entire_service_ = node->create_service<ClearEntirely>(
-    "clear_entirely_" + costmap_.getName(),
+  clear_entire_service_ = std::make_shared<nav2_util::ServiceServer<ClearEntirely,
+      std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>>(
+    std::string("clear_entirely_") + costmap_.getName(),
+    node,
     std::bind(
       &ClearCostmapService::clearEntireCallback, this,
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
@@ -135,8 +141,8 @@ void ClearCostmapService::clearLayerRegion(
   double end_point_y = start_point_y + reset_distance;
 
   int start_x, start_y, end_x, end_y;
-  costmap->worldToMapEnforceBounds(start_point_x, start_point_y, start_x, start_y);
-  costmap->worldToMapEnforceBounds(end_point_x, end_point_y, end_x, end_y);
+  costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
+  costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
 
   costmap->clearArea(start_x, start_y, end_x, end_y, invert);
 
diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp
index 1d9997de8da..f3903308cff 100644
--- a/nav2_costmap_2d/src/costmap_2d.cpp
+++ b/nav2_costmap_2d/src/costmap_2d.cpp
@@ -282,6 +282,12 @@ void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double & wx, double
   wy = origin_y_ + (my + 0.5) * resolution_;
 }
 
+void Costmap2D::mapToWorldNoBounds(int mx, int my, double & wx, double & wy) const
+{
+  wx = origin_x_ + (mx + 0.5) * resolution_;
+  wy = origin_y_ + (my + 0.5) * resolution_;
+}
+
 bool Costmap2D::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const
 {
   if (wx < origin_x_ || wy < origin_y_) {
@@ -303,8 +309,8 @@ bool Costmap2D::worldToMapContinuous(double wx, double wy, float & mx, float & m
     return false;
   }
 
-  mx = static_cast<float>((wx - origin_x_) / resolution_) + 0.5f;
-  my = static_cast<float>((wy - origin_y_) / resolution_) + 0.5f;
+  mx = static_cast<float>((wx - origin_x_) / resolution_);
+  my = static_cast<float>((wy - origin_y_) / resolution_);
 
   if (mx < size_x_ && my < size_y_) {
     return true;
diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp
index 484ceb14225..113ed927651 100644
--- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp
@@ -82,11 +82,13 @@ Costmap2DPublisher::Costmap2DPublisher(
     topic_name + "_raw_updates", custom_qos);
 
   // Create a service that will use the callback function to handle requests.
-  costmap_service_ = node->create_service<nav2_msgs::srv::GetCostmap>(
-    "get_" + topic_name, std::bind(
-      &Costmap2DPublisher::costmap_service_callback,
-      this, std::placeholders::_1, std::placeholders::_2,
-      std::placeholders::_3));
+  costmap_service_ = std::make_shared<nav2_util::ServiceServer<nav2_msgs::srv::GetCostmap,
+      std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>>(
+    std::string("get_") + topic_name,
+    node,
+    std::bind(
+      &Costmap2DPublisher::costmap_service_callback, this,
+      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
   if (cost_translation_table_ == NULL) {
     cost_translation_table_ = new char[256];
@@ -154,7 +156,8 @@ void Costmap2DPublisher::prepareGrid()
   grid_->data.resize(grid_->info.width * grid_->info.height);
 
   unsigned char * data = costmap_->getCharMap();
-  std::transform(data, data + grid_->data.size(), grid_->data.begin(),
+  std::transform(
+    data, data + grid_->data.size(), grid_->data.begin(),
     [](unsigned char c) {return cost_translation_table_[c];});
 }
 
@@ -203,8 +206,9 @@ std::unique_ptr<map_msgs::msg::OccupancyGridUpdate> Costmap2DPublisher::createGr
   std::uint32_t i = 0;
   for (std::uint32_t y = y0_; y < yn_; y++) {
     std::uint32_t row_start = y * map_width + x0_;
-    std::transform(costmap_data + row_start, costmap_data + row_start + update->width,
-        update->data.begin() + i,
+    std::transform(
+      costmap_data + row_start, costmap_data + row_start + update->width,
+      update->data.begin() + i,
       [](unsigned char c) {return cost_translation_table_[c];});
     i += update->width;
   }
diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
index 98f5cbbef0c..55665368372 100644
--- a/nav2_costmap_2d/src/costmap_2d_ros.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -178,11 +178,13 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
 
     // TODO(mjeronimo): instead of get(), use a shared ptr
     try {
-      plugin->initialize(layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(),
-          shared_from_this(), callback_group_);
+      plugin->initialize(
+        layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(),
+        shared_from_this(), callback_group_);
     } catch (const std::exception & e) {
-      RCLCPP_ERROR(get_logger(), "Failed to initialize costmap plugin %s! %s.",
-          plugin_names_[i].c_str(), e.what());
+      RCLCPP_ERROR(
+        get_logger(), "Failed to initialize costmap plugin %s! %s.",
+        plugin_names_[i].c_str(), e.what());
       return nav2_util::CallbackReturn::FAILURE;
     }
 
@@ -248,8 +250,10 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
   }
 
   // Service to get the cost at a point
-  get_cost_service_ = create_service<nav2_msgs::srv::GetCosts>(
-    "get_cost_" + getName(),
+  get_cost_service_ = std::make_shared<nav2_util::ServiceServer<nav2_msgs::srv::GetCosts,
+      std::shared_ptr<nav2_util::LifecycleNode>>>(
+    std::string("get_cost_") + get_name(),
+    shared_from_this(),
     std::bind(
       &Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3));
@@ -360,7 +364,7 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Cleaning up");
   executor_thread_.reset();
-
+  get_cost_service_.reset();
   costmap_publisher_.reset();
   clear_costmap_service_.reset();
 
@@ -830,6 +834,7 @@ void Costmap2DROS::getCostsCallback(
   unsigned int mx, my;
 
   Costmap2D * costmap = layered_costmap_->getCostmap();
+  std::unique_lock<Costmap2D::mutex_t> lock(*(costmap->getMutex()));
   response->success = true;
   for (const auto & pose : request->poses) {
     geometry_msgs::msg::PoseStamped pose_transformed;
@@ -858,7 +863,7 @@ void Costmap2DROS::getCostsCallback(
     } else {
       RCLCPP_DEBUG(
         get_logger(), "Received request to get cost at point (%f, %f)",
-          pose_transformed.pose.position.x,
+        pose_transformed.pose.position.x,
         pose_transformed.pose.position.y);
 
       bool in_bounds = costmap->worldToMap(
diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp
index 93f20f664dc..72a0120cd58 100644
--- a/nav2_costmap_2d/src/costmap_layer.cpp
+++ b/nav2_costmap_2d/src/costmap_layer.cpp
@@ -66,10 +66,19 @@ void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y, boo
 {
   current_ = false;
   unsigned char * grid = getCharMap();
-  for (int x = 0; x < static_cast<int>(getSizeInCellsX()); x++) {
+
+  int size_x = getSizeInCellsX();
+  int size_y = getSizeInCellsY();
+
+  start_x = std::clamp(start_x, 0, size_x);
+  start_y = std::clamp(start_y, 0, size_y);
+  end_x = std::clamp(end_x, 0, size_x);
+  end_y = std::clamp(end_y, 0, size_y);
+
+  for (int x = 0; x < size_x; x++) {
     bool xrange = x > start_x && x < end_x;
 
-    for (int y = 0; y < static_cast<int>(getSizeInCellsY()); y++) {
+    for (int y = 0; y < size_y; y++) {
       if ((xrange && y > start_y && y < end_y) == invert) {
         continue;
       }
diff --git a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp
index 11168f63731..bd70e70442f 100644
--- a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp
+++ b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp
@@ -38,10 +38,23 @@ CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
   std::string name)
 : name_(name),
   costmap_sub_(costmap_sub),
-  footprint_sub_(footprint_sub),
+  footprint_sub_(&footprint_sub),
   collision_checker_(nullptr)
 {}
 
+CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
+  CostmapSubscriber & costmap_sub,
+  std::string footprint_string,
+  std::string name)
+: name_(name),
+  costmap_sub_(costmap_sub),
+  collision_checker_(nullptr)
+{
+  if (!makeFootprintFromString(footprint_string, footprint_)) {
+    throw CollisionCheckerException("Failed to create footprint from string");
+  }
+}
+
 bool CostmapTopicCollisionChecker::isCollisionFree(
   const geometry_msgs::msg::Pose2D & pose,
   bool fetch_costmap_and_footprint)
@@ -90,7 +103,10 @@ Footprint CostmapTopicCollisionChecker::getFootprint(
 {
   if (fetch_latest_footprint) {
     std_msgs::msg::Header header;
-    if (!footprint_sub_.getFootprintInRobotFrame(footprint_, header)) {
+
+    // if footprint_sub_ was not initialized (alternative constructor), we are using the
+    // footprint built from the footprint_string alternative constructor argument.
+    if (footprint_sub_ && !footprint_sub_->getFootprintInRobotFrame(footprint_, header)) {
       throw CollisionCheckerException("Current footprint not available.");
     }
   }
diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt
index 490b4815e8b..c8ba5291340 100644
--- a/nav2_costmap_2d/test/integration/CMakeLists.txt
+++ b/nav2_costmap_2d/test/integration/CMakeLists.txt
@@ -165,4 +165,4 @@ ament_add_test(test_costmap_subscriber_exec
 #   nav2_util::nav2_utils_core
 #   rclcpp::rclcpp
 #   tf2_ros::tf2_ros
-# )
\ No newline at end of file
+# )
diff --git a/nav2_costmap_2d/test/integration/costmap_tests_launch.py b/nav2_costmap_2d/test/integration/costmap_tests_launch.py
index 928c4a14ec1..d1bec18587f 100755
--- a/nav2_costmap_2d/test/integration/costmap_tests_launch.py
+++ b/nav2_costmap_2d/test/integration/costmap_tests_launch.py
@@ -17,18 +17,16 @@
 import os
 import sys
 
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import ExecuteProcess
-from launch.actions import IncludeLaunchDescription
+from launch import LaunchDescription, LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 import launch_ros.actions
 from launch_testing.legacy import LaunchTestService
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]) -> LaunchTestService:
     launchFile = os.path.join(
-        os.getenv('TEST_LAUNCH_DIR'), 'costmap_map_server.launch.py'
+        os.getenv('TEST_LAUNCH_DIR', ''), 'costmap_map_server.launch.py'
     )
     testExecutable = os.getenv('TEST_EXECUTABLE')
 
diff --git a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp
index 87da2d52fc2..7376af89ecf 100644
--- a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp
+++ b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp
@@ -98,7 +98,7 @@ TEST(DynParamTestNode, testDynParamsSet)
   costmap->on_shutdown(rclcpp_lifecycle::State());
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_costmap_2d/test/integration/footprint_tests.cpp b/nav2_costmap_2d/test/integration/footprint_tests.cpp
index 2f527b5faac..74dab03aaa8 100644
--- a/nav2_costmap_2d/test/integration/footprint_tests.cpp
+++ b/nav2_costmap_2d/test/integration/footprint_tests.cpp
@@ -195,7 +195,7 @@ TEST_F(TestNode, footprint_from_same_level_param)
   EXPECT_EQ(0.0f, footprint[2].z);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp
index 3c266192248..0a18e2e771f 100644
--- a/nav2_costmap_2d/test/integration/inflation_tests.cpp
+++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp
@@ -535,7 +535,7 @@ TEST_F(TestNode, testInflation2)
 
   waitForMap(slayer);
 
-  // Creat a small L-Shape all at once
+  // Create a small L-Shape all at once
   addObservation(olayer, 1, 1, MAX_Z);
   addObservation(olayer, 2, 1, MAX_Z);
   addObservation(olayer, 2, 2, MAX_Z);
@@ -632,7 +632,7 @@ TEST_F(TestNode, testDynParamsSet)
   costmap->on_shutdown(rclcpp_lifecycle::State());
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp
index 7dd0b4eb3a9..fc06a3de82c 100644
--- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp
+++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp
@@ -188,7 +188,7 @@ TEST_F(TestNode, testRaytracing2) {
   ASSERT_EQ(obs_before, 20);
 
   // The sensor origin will be <0,0>. So if we add an obstacle at 9,9,
-  // we would expect cells <0, 0> thru <8, 8> to be traced through
+  // we would expect cells <0, 0> through <8, 8> to be traced through
   // however the static map is not cleared by obstacle layer
   addObservation(olayer, 9.5, 9.5, MAX_Z / 2, 0.5, 0.5, MAX_Z / 2);
   layers.updateMap(0, 0, 0);
@@ -618,7 +618,7 @@ TEST_F(TestNodeWithoutUnknownOverwrite, testMaxWithoutUnknownOverwriteCombinatio
   ASSERT_EQ(unknown_count, 100);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp
index 06e15ea6e6a..063c53de8d0 100644
--- a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp
+++ b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp
@@ -82,7 +82,8 @@ class TestNode : public ::testing::Test
     node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false));
     node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false));
     node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
-    node_->declare_parameter("unknown_cost_value",
+    node_->declare_parameter(
+      "unknown_cost_value",
       rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));
     node_->declare_parameter("trinary_costmap", rclcpp::ParameterValue(true));
     node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3));
@@ -188,7 +189,8 @@ TEST_F(TestNode, testObstacleLayers) {
 TEST_F(TestNode, testObstacleAndStaticLayers) {
   tf2_ros::Buffer tf(node_->get_clock());
 
-  node_->declare_parameter("pclayer_a.static.map_topic",
+  node_->declare_parameter(
+    "pclayer_a.static.map_topic",
     rclcpp::ParameterValue(std::string("map")));
 
   nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
@@ -223,13 +225,16 @@ TEST_F(TestNode, testObstacleAndStaticLayers) {
 TEST_F(TestNode, testDifferentInflationLayers) {
   tf2_ros::Buffer tf(node_->get_clock());
 
-  node_->declare_parameter("pclayer_a.static.map_topic",
+  node_->declare_parameter(
+    "pclayer_a.static.map_topic",
     rclcpp::ParameterValue(std::string("map")));
 
-  node_->declare_parameter("pclayer_b.inflation.cost_scaling_factor",
+  node_->declare_parameter(
+    "pclayer_b.inflation.cost_scaling_factor",
     rclcpp::ParameterValue(1.0));
 
-  node_->declare_parameter("pclayer_b.inflation.inflation_radius",
+  node_->declare_parameter(
+    "pclayer_b.inflation.inflation_radius",
     rclcpp::ParameterValue(1.0));
 
   nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
@@ -272,13 +277,16 @@ TEST_F(TestNode, testDifferentInflationLayers) {
 TEST_F(TestNode, testDifferentInflationLayers2) {
   tf2_ros::Buffer tf(node_->get_clock());
 
-  node_->declare_parameter("pclayer_a.static.map_topic",
+  node_->declare_parameter(
+    "pclayer_a.static.map_topic",
     rclcpp::ParameterValue(std::string("map")));
 
-  node_->declare_parameter("pclayer_a.inflation.cost_scaling_factor",
+  node_->declare_parameter(
+    "pclayer_a.inflation.cost_scaling_factor",
     rclcpp::ParameterValue(1.0));
 
-  node_->declare_parameter("pclayer_a.inflation.inflation_radius",
+  node_->declare_parameter(
+    "pclayer_a.inflation.inflation_radius",
     rclcpp::ParameterValue(1.0));
 
   nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
@@ -321,19 +329,24 @@ TEST_F(TestNode, testDifferentInflationLayers2) {
 TEST_F(TestNode, testResetting) {
   tf2_ros::Buffer tf(node_->get_clock());
 
-  node_->declare_parameter("pclayer_a.static.map_topic",
+  node_->declare_parameter(
+    "pclayer_a.static.map_topic",
     rclcpp::ParameterValue(std::string("map")));
 
-  node_->declare_parameter("pclayer_a.inflation.cost_scaling_factor",
+  node_->declare_parameter(
+    "pclayer_a.inflation.cost_scaling_factor",
     rclcpp::ParameterValue(1.0));
 
-  node_->declare_parameter("pclayer_a.inflation.inflation_radius",
+  node_->declare_parameter(
+    "pclayer_a.inflation.inflation_radius",
     rclcpp::ParameterValue(1.0));
 
-  node_->declare_parameter("pclayer_b.inflation.cost_scaling_factor",
+  node_->declare_parameter(
+    "pclayer_b.inflation.cost_scaling_factor",
     rclcpp::ParameterValue(1.0));
 
-  node_->declare_parameter("pclayer_b.inflation.inflation_radius",
+  node_->declare_parameter(
+    "pclayer_b.inflation.inflation_radius",
     rclcpp::ParameterValue(1.0));
 
   nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
@@ -392,19 +405,24 @@ TEST_F(TestNode, testResetting) {
 TEST_F(TestNode, testClearing) {
   tf2_ros::Buffer tf(node_->get_clock());
 
-  node_->declare_parameter("pclayer_a.static.map_topic",
+  node_->declare_parameter(
+    "pclayer_a.static.map_topic",
     rclcpp::ParameterValue(std::string("map")));
 
-  node_->declare_parameter("pclayer_a.inflation.cost_scaling_factor",
+  node_->declare_parameter(
+    "pclayer_a.inflation.cost_scaling_factor",
     rclcpp::ParameterValue(1.0));
 
-  node_->declare_parameter("pclayer_a.inflation.inflation_radius",
+  node_->declare_parameter(
+    "pclayer_a.inflation.inflation_radius",
     rclcpp::ParameterValue(1.0));
 
-  node_->declare_parameter("pclayer_b.inflation.cost_scaling_factor",
+  node_->declare_parameter(
+    "pclayer_b.inflation.cost_scaling_factor",
     rclcpp::ParameterValue(1.0));
 
-  node_->declare_parameter("pclayer_b.inflation.inflation_radius",
+  node_->declare_parameter(
+    "pclayer_b.inflation.inflation_radius",
     rclcpp::ParameterValue(1.0));
 
   nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
@@ -458,19 +476,24 @@ TEST_F(TestNode, testClearing) {
 TEST_F(TestNode, testOverwriteCombinationMethods) {
   tf2_ros::Buffer tf(node_->get_clock());
 
-  node_->declare_parameter("pclayer_a.static.map_topic",
+  node_->declare_parameter(
+    "pclayer_a.static.map_topic",
     rclcpp::ParameterValue(std::string("map")));
 
-  node_->declare_parameter("pclayer_a.inflation.cost_scaling_factor",
+  node_->declare_parameter(
+    "pclayer_a.inflation.cost_scaling_factor",
     rclcpp::ParameterValue(1.0));
 
-  node_->declare_parameter("pclayer_a.inflation.inflation_radius",
+  node_->declare_parameter(
+    "pclayer_a.inflation.inflation_radius",
     rclcpp::ParameterValue(1.0));
 
-  node_->declare_parameter("pclayer_b.inflation.cost_scaling_factor",
+  node_->declare_parameter(
+    "pclayer_b.inflation.cost_scaling_factor",
     rclcpp::ParameterValue(1.0));
 
-  node_->declare_parameter("pclayer_b.inflation.inflation_radius",
+  node_->declare_parameter(
+    "pclayer_b.inflation.inflation_radius",
     rclcpp::ParameterValue(1.0));
 
   nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
@@ -519,19 +542,24 @@ TEST_F(TestNode, testOverwriteCombinationMethods) {
 TEST_F(TestNode, testWithoutUnknownOverwriteCombinationMethods) {
   tf2_ros::Buffer tf(node_->get_clock());
 
-  node_->declare_parameter("pclayer_a.static.map_topic",
+  node_->declare_parameter(
+    "pclayer_a.static.map_topic",
     rclcpp::ParameterValue(std::string("map")));
 
-  node_->declare_parameter("pclayer_a.inflation.cost_scaling_factor",
+  node_->declare_parameter(
+    "pclayer_a.inflation.cost_scaling_factor",
     rclcpp::ParameterValue(1.0));
 
-  node_->declare_parameter("pclayer_a.inflation.inflation_radius",
+  node_->declare_parameter(
+    "pclayer_a.inflation.inflation_radius",
     rclcpp::ParameterValue(1.0));
 
-  node_->declare_parameter("pclayer_b.inflation.cost_scaling_factor",
+  node_->declare_parameter(
+    "pclayer_b.inflation.cost_scaling_factor",
     rclcpp::ParameterValue(1.0));
 
-  node_->declare_parameter("pclayer_b.inflation.inflation_radius",
+  node_->declare_parameter(
+    "pclayer_b.inflation.inflation_radius",
     rclcpp::ParameterValue(1.0));
 
   nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
@@ -616,31 +644,39 @@ TEST_F(TestNode, testDynParamsSetPluginContainerLayer)
   plugins_str.push_back("plugin_container_layer_b");
   costmap->set_parameter(rclcpp::Parameter("plugins", plugins_str));
 
-  costmap->declare_parameter("plugin_container_layer_a.plugin",
+  costmap->declare_parameter(
+    "plugin_container_layer_a.plugin",
     rclcpp::ParameterValue(std::string("nav2_costmap_2d::PluginContainerLayer")));
-  costmap->declare_parameter("plugin_container_layer_b.plugin",
+  costmap->declare_parameter(
+    "plugin_container_layer_b.plugin",
     rclcpp::ParameterValue(std::string("nav2_costmap_2d::PluginContainerLayer")));
 
   std::vector<std::string> plugins_str_a;
   plugins_str_a.push_back("obstacle_layer");
   plugins_str_a.push_back("inflation_layer");
-  costmap->declare_parameter("plugin_container_layer_a.plugins",
+  costmap->declare_parameter(
+    "plugin_container_layer_a.plugins",
     rclcpp::ParameterValue(plugins_str_a));
 
-  costmap->declare_parameter("plugin_container_layer_a.obstacle_layer.plugin",
+  costmap->declare_parameter(
+    "plugin_container_layer_a.obstacle_layer.plugin",
     rclcpp::ParameterValue(std::string("nav2_costmap_2d::ObstacleLayer")));
-  costmap->declare_parameter("plugin_container_layer_a.inflation_layer.plugin",
+  costmap->declare_parameter(
+    "plugin_container_layer_a.inflation_layer.plugin",
     rclcpp::ParameterValue(std::string("nav2_costmap_2d::InflationLayer")));
 
   std::vector<std::string> plugins_str_b;
   plugins_str_b.push_back("static_layer");
   plugins_str_b.push_back("inflation_layer");
-  costmap->declare_parameter("plugin_container_layer_b.plugins",
+  costmap->declare_parameter(
+    "plugin_container_layer_b.plugins",
     rclcpp::ParameterValue(plugins_str_b));
 
-  costmap->declare_parameter("plugin_container_layer_b.static_layer.plugin",
+  costmap->declare_parameter(
+    "plugin_container_layer_b.static_layer.plugin",
     rclcpp::ParameterValue(std::string("nav2_costmap_2d::StaticLayer")));
-  costmap->declare_parameter("plugin_container_layer_b.inflation_layer.plugin",
+  costmap->declare_parameter(
+    "plugin_container_layer_b.inflation_layer.plugin",
     rclcpp::ParameterValue(std::string("nav2_costmap_2d::InflationLayer")));
 
 
@@ -679,7 +715,7 @@ TEST_F(TestNode, testDynParamsSetPluginContainerLayer)
   costmap->on_shutdown(rclcpp_lifecycle::State());
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_costmap_2d/test/integration/range_tests.cpp b/nav2_costmap_2d/test/integration/range_tests.cpp
index 1a14113922f..1738fb7cde0 100644
--- a/nav2_costmap_2d/test/integration/range_tests.cpp
+++ b/nav2_costmap_2d/test/integration/range_tests.cpp
@@ -277,7 +277,7 @@ TEST_F(TestNode, testProbabilisticModelDownward) {
   ASSERT_EQ(layers.getCostmap()->getCost(3, 7), 254);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp
index a89d24a7da7..53d0816c3a3 100644
--- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp
+++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp
@@ -171,7 +171,7 @@ TEST_F(CostmapRosTestFixture, costmap_pub_test)
   SUCCEED();
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp
index 6fcabadd5f3..b2e0e94d270 100644
--- a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp
+++ b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp
@@ -95,7 +95,7 @@ class TestCostmapSubscriberShould : public ::testing::Test
   {
     this->updateCostmapMsgCount++;
     std::vector<uint8_t> data;
-    for(unsigned int i = 0; i < update_msg->data.size(); i++) {
+    for (unsigned int i = 0; i < update_msg->data.size(); i++) {
       data.push_back(update_msg->data[i]);
     }
     receivedGrids.push_back(data);
@@ -256,7 +256,7 @@ TEST_F(TestCostmapSubscriberShould, handleCostmapUpdateMsgs)
       costmapToSend->setCost(observation.x, observation.y, observation.cost);
     }
     // Publish full costmap for the first iteration
-    if(!first_iteration) {
+    if (!first_iteration) {
       x0 = mapChange.x0;
       xn = mapChange.xn;
       y0 = mapChange.y0;
@@ -294,7 +294,7 @@ TEST_F(
   ASSERT_ANY_THROW(costmapSubscriber->getCostmap());
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp
index 6fa648ddc8f..6032d9c72f2 100644
--- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp
+++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp
@@ -355,7 +355,7 @@ TEST_F(TestNode, CollisionSpace)
   ASSERT_EQ(collision_checker_->testPose(4.5, 4.5, 0), false);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_costmap_2d/test/module_tests.cpp b/nav2_costmap_2d/test/module_tests.cpp
index 5f368144b58..fb8fa9eb388 100644
--- a/nav2_costmap_2d/test/module_tests.cpp
+++ b/nav2_costmap_2d/test/module_tests.cpp
@@ -915,7 +915,7 @@ TEST(costmap, testInflation2) {
     ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
     10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
 
-  // Creat a small L-Shape all at once
+  // Create a small L-Shape all at once
   pcl::PointCloud<pcl::PointXYZ> c0;
   c0.points.resize(3);
   c0.points[0].x = 1;
@@ -1039,7 +1039,7 @@ TEST(costmap, testRaytracing2) {
     100.0, MAX_Z, 100.0, 1, MAP_10_BY_10, THRESHOLD);
 
   // The sensor origin will be <0,0>. So if we add an obstacle at 9,9, we would expect cells
-  // <0, 0> thru <8, 8> to be traced through
+  // <0, 0> through <8, 8> to be traced through
   pcl::PointCloud<pcl::PointXYZ> c0;
   c0.points.resize(1);
   c0.points[0].x = 9.5;
diff --git a/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp b/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp
index a45cb38a7f9..c4afee34e00 100644
--- a/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp
+++ b/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp
@@ -100,7 +100,7 @@ TEST(costmap_2d, bresenham2DBoundariesCheck)
   CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0);
   CostmapAction ca(ct.getCostmap(), ct.getSize());
 
-  // Initial point - some assymetrically standing point in order to cover most corner cases
+  // Initial point - some asymmetrically standing point in order to cover most corner cases
   const unsigned int x0 = 2;
   const unsigned int y0 = 4;
   // (x1, y1) point will move
diff --git a/nav2_costmap_2d/test/test_launch_files/costmap_map_server.launch.py b/nav2_costmap_2d/test/test_launch_files/costmap_map_server.launch.py
index 622d55e0745..b3ff9b09beb 100644
--- a/nav2_costmap_2d/test/test_launch_files/costmap_map_server.launch.py
+++ b/nav2_costmap_2d/test/test_launch_files/costmap_map_server.launch.py
@@ -20,7 +20,7 @@
 import launch_ros.actions
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     mapFile = os.getenv('TEST_MAP')
     return LaunchDescription(
         [
diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt
index 291ffb062f9..559ef4ae4e2 100644
--- a/nav2_costmap_2d/test/unit/CMakeLists.txt
+++ b/nav2_costmap_2d/test/unit/CMakeLists.txt
@@ -62,3 +62,8 @@ ament_add_gtest(lifecycle_test lifecycle_test.cpp)
 target_link_libraries(lifecycle_test
   nav2_costmap_2d_core
 )
+
+ament_add_gtest(coordinate_transform_test coordinate_transform_test.cpp)
+target_link_libraries(coordinate_transform_test
+  nav2_costmap_2d_core
+)
diff --git a/nav2_costmap_2d/test/unit/coordinate_transform_test.cpp b/nav2_costmap_2d/test/unit/coordinate_transform_test.cpp
new file mode 100644
index 00000000000..e81698b14d5
--- /dev/null
+++ b/nav2_costmap_2d/test/unit/coordinate_transform_test.cpp
@@ -0,0 +1,59 @@
+// Copyright (c) 2025 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <gtest/gtest.h>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+
+
+/**
+ * Test for mapToWorldNoBounds
+ */
+
+TEST(mapToWorldNoBounds, MapToWorldNoBoundsNegativeMapCoords)
+{
+  double wx, wy;
+
+  std::unique_ptr<nav2_costmap_2d::Costmap2D> map;
+
+  map = std::make_unique<nav2_costmap_2d::Costmap2D>(10, 10, 1.0, 0.0, 0.0);
+  map->mapToWorldNoBounds(-1, -1, wx, wy);
+  EXPECT_DOUBLE_EQ(wx, -0.5);
+  EXPECT_DOUBLE_EQ(wy, -0.5);
+
+  map = std::make_unique<nav2_costmap_2d::Costmap2D>(10, 10, 1.0, 1.0, 2.0);
+  map->mapToWorldNoBounds(-5, -5, wx, wy);
+  EXPECT_DOUBLE_EQ(wx, -3.5);
+  EXPECT_DOUBLE_EQ(wy, -2.5);
+
+  map = std::make_unique<nav2_costmap_2d::Costmap2D>(10, 10, 2.0, 3.0, 4.0);
+  map->mapToWorldNoBounds(-10, -10, wx, wy);
+  EXPECT_DOUBLE_EQ(wx, -16.0);
+  EXPECT_DOUBLE_EQ(wy, -15.0);
+}
+
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  rclcpp::init(0, nullptr);
+
+  int result = RUN_ALL_TESTS();
+
+  rclcpp::shutdown();
+
+  return result;
+}
diff --git a/nav2_costmap_2d/test/unit/copy_window_test.cpp b/nav2_costmap_2d/test/unit/copy_window_test.cpp
index 57ac89ce3b9..ecc952f43ee 100644
--- a/nav2_costmap_2d/test/unit/copy_window_test.cpp
+++ b/nav2_costmap_2d/test/unit/copy_window_test.cpp
@@ -43,7 +43,7 @@ TEST(CopyWindow, copyInvalidWindow)
   ASSERT_FALSE(dst.copyWindow(src, 0, 0, 6, 6, 0, 0));
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_costmap_2d/test/unit/costmap_conversion_test.cpp b/nav2_costmap_2d/test/unit/costmap_conversion_test.cpp
index aea641c7212..a1d62142e73 100644
--- a/nav2_costmap_2d/test/unit/costmap_conversion_test.cpp
+++ b/nav2_costmap_2d/test/unit/costmap_conversion_test.cpp
@@ -113,7 +113,7 @@ TEST_F(TestNode, convertOccGridToCostmap)
   verifyCostmap();
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp
index 3e82f51fcc9..6a769a8cf3b 100644
--- a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp
+++ b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp
@@ -86,7 +86,7 @@ TEST_F(GetCostServiceTest, TestWithFootprint)
   }
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp
index 4106936cef1..377df675085 100644
--- a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp
+++ b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp
@@ -66,7 +66,7 @@ TEST(DeclareParameter, useInvalidParameter)
   }
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_costmap_2d/test/unit/denoise_layer_test.cpp b/nav2_costmap_2d/test/unit/denoise_layer_test.cpp
index 3f1a89cf212..599959ba856 100644
--- a/nav2_costmap_2d/test/unit/denoise_layer_test.cpp
+++ b/nav2_costmap_2d/test/unit/denoise_layer_test.cpp
@@ -509,7 +509,7 @@ TEST_F(DenoiseLayerTester, initializeInvalid) {
     std::make_tuple(true, ConnectivityType::Way8, 1));
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_docking/README.md b/nav2_docking/README.md
index 0352ee95aef..b063e72dd75 100644
--- a/nav2_docking/README.md
+++ b/nav2_docking/README.md
@@ -26,7 +26,7 @@ Want to learn more? Checkout the ROSCon 2024 talk on Docking by clicking on the
 
 The Docking Framework has 5 main components:
 - `DockingServer`: The main action server and logic for performing the docking/undocking actions
-- `Navigator`: A NavigateToPose action client to navigate the robot to the dock's staging pose if not with the prestaging tolerances 
+- `Navigator`: A NavigateToPose action client to navigate the robot to the dock's staging pose if not with the prestaging tolerances
 - `DockDatabase`: A database of dock instances in an environment and their associated interfaces for transacting with each type. An arbitrary number of types are supported.
 - `Controller`: A spiral-based graceful controller to use for the vision-control loop for docking
 - `ChargingDock` and `NonChargingDock`: Plugins that describe the dock and how to transact with it (check if charging, detection, etc). You can find these plugin headers in the `opennav_docking_core` package.
@@ -38,7 +38,7 @@ The docking procedure is as follows:
 2. If the robot is not within the prestaging tolerance of the dock's staging pose, navigate to the staging pose
 3. Use the dock's plugin to initially detect the dock and return the docking pose
 4. Enter a vision-control loop where the robot attempts to reach the docking pose while its actively being refined by the vision system
-5. Exit the vision-control loop once contact has been detected or charging has started 
+5. Exit the vision-control loop once contact has been detected or charging has started
 6. Wait until charging starts (if applicable) and return success.
 
 If anywhere this procedure is unsuccessful, `N` retries may be made by driving back to the dock's staging pose and trying again. If still unsuccessful, it will return a failure code to indicate what kind of failure occurred to the client.
@@ -48,19 +48,19 @@ Undocking works more simply:
 2. Find the staging pose for this dock and back out to that pose
 3. Check if successfully backed out to the pose and charging has stopped
 
-## Interfaces 
+## Interfaces
 
 ### Docking Action
 
-The docking action can either operate on a dock in the `DockDatabase` or from a dock specified in the docking request. This second option is useful for testing or when dock's locales are not necessarily known in advance. 
+The docking action can either operate on a dock in the `DockDatabase` or from a dock specified in the docking request. This second option is useful for testing or when dock's locales are not necessarily known in advance.
 If `use_dock_id = true`, it uses the `dock_id` field to specify which dock in the database to use.
 Else, you must populate the `dock_pose` and `dock_type` fields.
 
 If you wish for the docking server to stage your robot at the the dock's staging pose for you, `navigate_to_staging_pose` must be true.
-Else, you can send your robot to this pose and it will be skipped as long as the robot is within the prestaging tolerances. 
+Else, you can send your robot to this pose and it will be skipped as long as the robot is within the prestaging tolerances.
 You may set the maximum time for navigation using `max_staging_time`.
 
-In return, you obtain the `num_retries`, for the number of attempted retries of the action; `success`, if the action worked and the robot is successfully charging; and `error_code` to return a semantically meaningful error code about what kind of error occurred, if any. See `DockRobot.action` for more details. 
+In return, you obtain the `num_retries`, for the number of attempted retries of the action; `success`, if the action worked and the robot is successfully charging; and `error_code` to return a semantically meaningful error code about what kind of error occurred, if any. See `DockRobot.action` for more details.
 
 While the action is performing, you can obtain feedback about the current `state` of docking, how much time `docking_time` has elapsed, and the current number of retries attempted.
 
@@ -174,7 +174,7 @@ some robots.
 
 `getStagingPose` applies a parameterized translational and rotational offset to the dock pose to obtain the staging pose.
 
-`getRefinedPose` can be used in two ways. 
+`getRefinedPose` can be used in two ways.
 1. A blind approach where the returned dock pose will simply be equal to whatever was passed in from the dock database. This may work with a reduced success rate on a real robot (due to global localization error), but is useful for initial testing and simulation.
 2. The more realistic use case is to use an AR marker, dock pose detection algorithm, etc. The plugin will subscribe to a `geometry_msgs/PoseStamped` topic `detected_dock_pose`. This can be used with the `image_proc/TrackMarkerNode` for Apriltags or other custom detectors for your dock. It is unlikely the detected pose is actually the pose you want to dock with, so several parameters are supplied to represent your docked pose relative to the detected feature's pose.
 
@@ -207,7 +207,6 @@ For debugging purposes, there are several publishers which can be used with RVIZ
 | max_retries        | Maximum number of retries to attempt    | int |  3      |
 | base_frame        | Robot's base frame for control law   | string |  "base_link"      |
 | fixed_frame        | Fixed frame to use, recommended to be a smooth odometry frame **not** map   | string |  "odom"      |
-| dock_backwards        | Whether the robot is docking with the dock forward or backward in motion | bool |  false      |
 | dock_prestaging_tolerance  |  L2 distance in X,Y,Theta from the staging pose to bypass navigation | double |  0.5      |
 | dock_plugins  | A set of dock plugins to load | vector<string> |  N/A      |
 | dock_database  |  The filepath to the dock database to use for this environment | string |  N/A  |
@@ -251,6 +250,7 @@ Note: `dock_plugins` and either `docks` or `dock_database` are required.
 | docking_threshold        | If not using stall detection, the pose threshold to the docking pose where `isDocked() = true`    | double |  0.05     |
 | staging_x_offset        | Staging pose offset forward (negative) of dock pose (m)    | double |  -0.7    |
 | staging_yaw_offset        | Staging pose angle relative to dock pose (rad)    | double |  0.0    |
+| dock_direction        | Whether the robot is docking with the dock forward or backward in motion | string |  "forward"  or "backward"    |
 
 Note: The external detection rotation angles are setup to work out of the box with Apriltags detectors in `image_proc` and `isaac_ros`.
 
@@ -259,4 +259,3 @@ Note: The external detection rotation angles are setup to work out of the box wi
 ### On Staging Poses
 
 Staging poses are where the robot should navigate to in order to start the docking procedure. This pose should be close enough to the dock to accurately detect the dock's presence, but far enough that if its moved slightly or the robot's localization isn't perfect it can still be detected and have enough room to adjust. The robot's charging contacts or charging location should be pointed towards the dock in this staging pose. That way, a feasible global planner can be used to model your robot's real constraints while getting to the docking pose (non-circular, non-holonomic), rather than complicating the docking process itself.
-
diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt
index add25f47544..508091931b6 100644
--- a/nav2_docking/opennav_docking/CMakeLists.txt
+++ b/nav2_docking/opennav_docking/CMakeLists.txt
@@ -33,8 +33,8 @@ add_library(controller SHARED
 )
 target_include_directories(controller
   PUBLIC
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
 target_link_libraries(controller PUBLIC
   ${geometry_msgs_TARGETS}
@@ -51,8 +51,8 @@ add_library(${library_name} SHARED
 )
 target_include_directories(${library_name}
   PUBLIC
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
 target_link_libraries(${library_name} PUBLIC
   angles::angles
@@ -79,8 +79,8 @@ add_library(pose_filter SHARED
 )
 target_include_directories(pose_filter
   PUBLIC
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
 target_link_libraries(pose_filter PUBLIC
   ${geometry_msgs_TARGETS}
@@ -95,8 +95,8 @@ add_executable(${executable_name}
 )
 target_include_directories(${executable_name}
   PUBLIC
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
 target_link_libraries(${executable_name} PRIVATE ${library_name} rclcpp::rclcpp)
 
@@ -107,8 +107,8 @@ add_library(simple_charging_dock SHARED
 )
 target_include_directories(simple_charging_dock
   PUBLIC
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
 target_link_libraries(simple_charging_dock PUBLIC
   ${geometry_msgs_TARGETS}
@@ -121,6 +121,7 @@ target_link_libraries(simple_charging_dock PUBLIC
   tf2_ros::tf2_ros
 )
 target_link_libraries(simple_charging_dock PRIVATE
+  angles::angles
   nav2_util::nav2_util_core
   pluginlib::pluginlib
   tf2::tf2
@@ -131,8 +132,8 @@ add_library(simple_non_charging_dock SHARED
 )
 target_include_directories(simple_non_charging_dock
   PUBLIC
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
 target_link_libraries(simple_non_charging_dock PUBLIC
   ${geometry_msgs_TARGETS}
@@ -145,6 +146,7 @@ target_link_libraries(simple_non_charging_dock PUBLIC
   tf2_ros::tf2_ros
 )
 target_link_libraries(simple_non_charging_dock PRIVATE
+  angles::angles
   nav2_util::nav2_util_core
   pluginlib::pluginlib
   tf2::tf2
@@ -167,7 +169,7 @@ install(DIRECTORY include/
   DESTINATION include/${PROJECT_NAME}
 )
 
-install(FILES test/test_dock_file.yaml
+install(DIRECTORY test/dock_files
   DESTINATION share/${PROJECT_NAME}/
 )
 
diff --git a/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp b/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp
index e6bcf96eac0..d151992ea42 100644
--- a/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp
+++ b/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp
@@ -23,7 +23,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
-
+#include "nav2_util/service_server.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_util/node_utils.hpp"
 #include "nav2_util/simple_action_server.hpp"
@@ -124,6 +124,7 @@ class DockDatabase
    * @param response Service response
    */
   void reloadDbCb(
+    const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Request> request,
     std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Response> response);
 
@@ -132,7 +133,8 @@ class DockDatabase
   DockPluginMap dock_plugins_;
   DockMap dock_instances_;
   pluginlib::ClassLoader<opennav_docking_core::ChargingDock> dock_loader_;
-  rclcpp::Service<nav2_msgs::srv::ReloadDockDatabase>::SharedPtr reload_db_service_;
+  nav2_util::ServiceServer<nav2_msgs::srv::ReloadDockDatabase,
+    std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>::SharedPtr reload_db_service_;
 };
 
 }  // namespace opennav_docking
diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp
index cd2ad0c5803..74d46f48f17 100644
--- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp
+++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp
@@ -15,11 +15,12 @@
 #ifndef OPENNAV_DOCKING__DOCKING_SERVER_HPP_
 #define OPENNAV_DOCKING__DOCKING_SERVER_HPP_
 
-#include <vector>
+#include <functional>
 #include <memory>
-#include <string>
 #include <mutex>
-#include <functional>
+#include <optional>
+#include <string>
+#include <vector>
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/lifecycle_node.hpp"
@@ -87,10 +88,11 @@ class DockingServer : public nav2_util::LifecycleNode
    * @brief Use control law and dock perception to approach the charge dock.
    * @param dock Dock instance, gets queried for refined pose and docked state.
    * @param dock_pose Initial dock pose, will be refined by perception.
+   * @param backward If true, the robot will drive backwards.
    * @returns True if dock successfully approached, False if cancelled. For
    *          any internal error, will throw.
    */
-  bool approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose);
+  bool approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose, bool backward);
 
   /**
    * @brief Wait for charging to begin.
@@ -102,9 +104,10 @@ class DockingServer : public nav2_util::LifecycleNode
   /**
    * @brief Reset the robot for another approach by controlling back to staging pose.
    * @param staging_pose The target pose that will reset for another approach.
+   * @param backward If true, the robot will drive backwards.
    * @returns True if reset is successful.
    */
-  bool resetApproach(const geometry_msgs::msg::PoseStamped & staging_pose);
+  bool resetApproach(const geometry_msgs::msg::PoseStamped & staging_pose, bool backward);
 
   /**
    * @brief Run a single iteration of the control loop to approach a pose.
@@ -242,7 +245,7 @@ class DockingServer : public nav2_util::LifecycleNode
   // This is our fixed frame for controlling - typically "odom"
   std::string fixed_frame_;
   // Does the robot drive backwards onto the dock? Default is forwards
-  bool dock_backwards_;
+  std::optional<bool> dock_backwards_;
   // The tolerance to the dock's staging pose not requiring navigation
   double dock_prestaging_tolerance_;
 
diff --git a/nav2_docking/opennav_docking/include/opennav_docking/utils.hpp b/nav2_docking/opennav_docking/include/opennav_docking/utils.hpp
index 1acd623f3ee..d0b3dcdf379 100644
--- a/nav2_docking/opennav_docking/include/opennav_docking/utils.hpp
+++ b/nav2_docking/opennav_docking/include/opennav_docking/utils.hpp
@@ -24,6 +24,7 @@
 #include "nav2_util/geometry_utils.hpp"
 #include "angles/angles.h"
 #include "opennav_docking/types.hpp"
+#include "opennav_docking_core/charging_dock.hpp"
 #include "tf2/utils.hpp"
 
 namespace utils
@@ -33,6 +34,7 @@ using rclcpp::ParameterType::PARAMETER_STRING;
 using rclcpp::ParameterType::PARAMETER_STRING_ARRAY;
 using rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY;
 using nav2_util::geometry_utils::orientationAroundZAxis;
+using opennav_docking_core::DockDirection;
 
 /**
 * @brief Parse a yaml file to obtain docks
@@ -183,6 +185,21 @@ inline double l2Norm(const geometry_msgs::msg::Pose & a, const geometry_msgs::ms
     delta_angle * delta_angle);
 }
 
+inline DockDirection getDockDirectionFromString(const std::string & direction)
+{
+  auto upper_direction = direction;
+  std::transform(
+    upper_direction.begin(), upper_direction.end(), upper_direction.begin(), ::toupper);
+
+  if (upper_direction == "FORWARD") {
+    return DockDirection::FORWARD;
+  } else if (upper_direction == "BACKWARD") {
+    return DockDirection::BACKWARD;
+  } else {
+    return DockDirection::UNKNOWN;
+  }
+}
+
 }  // namespace utils
 
 #endif  // OPENNAV_DOCKING__UTILS_HPP_
diff --git a/nav2_docking/opennav_docking/src/dock_database.cpp b/nav2_docking/opennav_docking/src/dock_database.cpp
index b6a1f888817..621cdd39f19 100644
--- a/nav2_docking/opennav_docking/src/dock_database.cpp
+++ b/nav2_docking/opennav_docking/src/dock_database.cpp
@@ -26,6 +26,7 @@ DockDatabase::~DockDatabase()
 {
   dock_instances_.clear();
   dock_plugins_.clear();
+  reload_db_service_.reset();
 }
 
 bool DockDatabase::initialize(
@@ -54,11 +55,13 @@ bool DockDatabase::initialize(
     "Docking Server has %u dock types and %u dock instances available.",
     this->plugin_size(), this->instance_size());
 
-  reload_db_service_ = node->create_service<nav2_msgs::srv::ReloadDockDatabase>(
+  reload_db_service_ = std::make_shared<nav2_util::ServiceServer<nav2_msgs::srv::ReloadDockDatabase,
+      std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>>(
     "~/reload_database",
+    node,
     std::bind(
       &DockDatabase::reloadDbCb, this,
-      std::placeholders::_1, std::placeholders::_2));
+      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
   return true;
 }
@@ -80,6 +83,7 @@ void DockDatabase::deactivate()
 }
 
 void DockDatabase::reloadDbCb(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
   const std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Request> request,
   std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Response> response)
 {
@@ -199,7 +203,7 @@ bool DockDatabase::getDockInstances(const rclcpp_lifecycle::LifecycleNode::Share
       node->get_logger(), "Loading dock from database file  %s.", dock_filepath.c_str());
     try {
       return utils::parseDockFile(dock_filepath, node, dock_instances_);
-    } catch (YAML::ParserException & e) {
+    } catch (YAML::BadConversion & e) {
       RCLCPP_ERROR(
         node->get_logger(),
         "Dock database (%s) is malformed: %s.", dock_filepath.c_str(), e.what());
diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp
index 3e85181fe87..3933addcb9c 100644
--- a/nav2_docking/opennav_docking/src/docking_server.cpp
+++ b/nav2_docking/opennav_docking/src/docking_server.cpp
@@ -38,7 +38,7 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options)
   declare_parameter("max_retries", 3);
   declare_parameter("base_frame", "base_link");
   declare_parameter("fixed_frame", "odom");
-  declare_parameter("dock_backwards", false);
+  declare_parameter("dock_backwards", rclcpp::PARAMETER_BOOL);
   declare_parameter("dock_prestaging_tolerance", 0.5);
 }
 
@@ -57,10 +57,20 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state)
   get_parameter("max_retries", max_retries_);
   get_parameter("base_frame", base_frame_);
   get_parameter("fixed_frame", fixed_frame_);
-  get_parameter("dock_backwards", dock_backwards_);
   get_parameter("dock_prestaging_tolerance", dock_prestaging_tolerance_);
   RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
 
+  bool dock_backwards;
+  try {
+    if (get_parameter("dock_backwards", dock_backwards)) {
+      dock_backwards_ = dock_backwards;
+      RCLCPP_WARN(
+        get_logger(), "Parameter dock_backwards is deprecated. "
+        "Please use the dock_direction parameter in your dock plugin instead.");
+    }
+  } catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
+  }
+
   vel_publisher_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel", 1);
   tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(node->get_clock());
 
@@ -155,6 +165,7 @@ DockingServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   curr_dock_type_.clear();
   controller_.reset();
   vel_publisher_.reset();
+  dock_backwards_.reset();
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -274,12 +285,17 @@ void DockingServer::dockRobot()
     doInitialPerception(dock, dock_pose);
     RCLCPP_INFO(get_logger(), "Successful initial dock detection");
 
+    // Get the direction of the movement
+    bool dock_backward = dock_backwards_.has_value() ?
+      dock_backwards_.value() :
+      (dock->plugin->getDockDirection() == opennav_docking_core::DockDirection::BACKWARD);
+
     // Docking control loop: while not docked, run controller
     rclcpp::Time dock_contact_time;
     while (rclcpp::ok()) {
       try {
         // Approach the dock using control law
-        if (approachDock(dock, dock_pose)) {
+        if (approachDock(dock, dock_pose, dock_backward)) {
           // We are docked, wait for charging to begin
           RCLCPP_INFO(
             get_logger(), "Made contact with dock, waiting for charge to start (if applicable).");
@@ -312,7 +328,7 @@ void DockingServer::dockRobot()
       }
 
       // Reset to staging pose to try again
-      if (!resetApproach(dock->getStagingPose())) {
+      if (!resetApproach(dock->getStagingPose(), dock_backward)) {
         // Cancelled, preempted, or shutting down
         stashDockData(goal->use_dock_id, dock, false);
         publishZeroVelocity();
@@ -409,11 +425,13 @@ void DockingServer::doInitialPerception(Dock * dock, geometry_msgs::msg::PoseSta
   }
 }
 
-bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose)
+bool DockingServer::approachDock(
+  Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose, bool backward)
 {
   rclcpp::Rate loop_rate(controller_frequency_);
   auto start = this->now();
   auto timeout = rclcpp::Duration::from_seconds(dock_approach_timeout_);
+
   while (rclcpp::ok()) {
     publishDockingFeedback(DockRobot::Feedback::CONTROLLING);
 
@@ -438,13 +456,6 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped &
     geometry_msgs::msg::PoseStamped target_pose = dock_pose;
     target_pose.header.stamp = rclcpp::Time(0);
 
-    // Make sure that the target pose is pointing at the robot when moving backwards
-    // This is to ensure that the robot doesn't try to dock from the wrong side
-    if (dock_backwards_) {
-      target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
-        tf2::getYaw(target_pose.pose.orientation) + M_PI);
-    }
-
     // The control law can get jittery when close to the end when atan2's can explode.
     // Thus, we backward project the controller's target pose a little bit after the
     // dock so that the robot never gets to the end of the spiral before its in contact
@@ -455,12 +466,17 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped &
     target_pose.pose.position.y += sin(yaw) * backward_projection;
     tf2_buffer_->transform(target_pose, target_pose, base_frame_);
 
+    // Make sure that the target pose is pointing at the robot when moving backwards
+    // This is to ensure that the robot doesn't try to dock from the wrong side
+    if (backward) {
+      target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
+        tf2::getYaw(target_pose.pose.orientation) + M_PI);
+    }
+
     // Compute and publish controls
     auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
     command->header.stamp = now();
-    if (!controller_->computeVelocityCommand(target_pose.pose, command->twist, true,
-        dock_backwards_))
-    {
+    if (!controller_->computeVelocityCommand(target_pose.pose, command->twist, true, backward)) {
       throw opennav_docking_core::FailedToControl("Failed to get control");
     }
     vel_publisher_->publish(std::move(command));
@@ -507,7 +523,8 @@ bool DockingServer::waitForCharge(Dock * dock)
   return false;
 }
 
-bool DockingServer::resetApproach(const geometry_msgs::msg::PoseStamped & staging_pose)
+bool DockingServer::resetApproach(
+  const geometry_msgs::msg::PoseStamped & staging_pose, bool backward)
 {
   rclcpp::Rate loop_rate(controller_frequency_);
   auto start = this->now();
@@ -527,7 +544,7 @@ bool DockingServer::resetApproach(const geometry_msgs::msg::PoseStamped & stagin
     command->header.stamp = now();
     if (getCommandToPose(
         command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, false,
-        !dock_backwards_))
+        !backward))
     {
       return true;
     }
@@ -619,9 +636,19 @@ void DockingServer::undockRobot()
       return;
     }
 
+    bool dock_backward = dock_backwards_.has_value() ?
+      dock_backwards_.value() :
+      (dock->getDockDirection() == opennav_docking_core::DockDirection::BACKWARD);
+
     // Get "dock pose" by finding the robot pose
     geometry_msgs::msg::PoseStamped dock_pose = getRobotPoseInFrame(fixed_frame_);
 
+    // Make sure that the staging pose is pointing in the same direction when moving backwards
+    if (dock_backward) {
+      dock_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
+        tf2::getYaw(dock_pose.pose.orientation) + M_PI);
+    }
+
     // Get staging pose (in fixed frame)
     geometry_msgs::msg::PoseStamped staging_pose =
       dock->getStagingPose(dock_pose.pose, dock_pose.header.frame_id);
@@ -653,9 +680,10 @@ void DockingServer::undockRobot()
       // Get command to approach staging pose
       auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
       command->header.stamp = now();
+
       if (getCommandToPose(
           command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, false,
-          !dock_backwards_))
+          !dock_backward))
       {
         RCLCPP_INFO(get_logger(), "Robot has reached staging pose");
         // Have reached staging_pose
diff --git a/nav2_docking/opennav_docking/src/navigator.cpp b/nav2_docking/opennav_docking/src/navigator.cpp
index e792411a75f..ff76c5eecd7 100644
--- a/nav2_docking/opennav_docking/src/navigator.cpp
+++ b/nav2_docking/opennav_docking/src/navigator.cpp
@@ -24,7 +24,8 @@ Navigator::Navigator(const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent)
 : node_(parent)
 {
   auto node = node_.lock();
-  node->declare_parameter("navigator_bt_xml", std::string(""));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "navigator_bt_xml", rclcpp::ParameterValue(std::string("")));
   node->get_parameter("navigator_bt_xml", navigator_bt_xml_);
 }
 
diff --git a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp
index 298a3be9504..10576a2afff 100644
--- a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp
+++ b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp
@@ -16,6 +16,7 @@
 
 #include "nav2_util/node_utils.hpp"
 #include "opennav_docking/simple_charging_dock.hpp"
+#include "opennav_docking/utils.hpp"
 
 namespace opennav_docking
 {
@@ -78,6 +79,10 @@ void SimpleChargingDock::configure(
   nav2_util::declare_parameter_if_not_declared(
     node_, name + ".staging_yaw_offset", rclcpp::ParameterValue(0.0));
 
+  // Direction of docking
+  nav2_util::declare_parameter_if_not_declared(
+    node_, name + ".dock_direction", rclcpp::ParameterValue(std::string("forward")));
+
   node_->get_parameter(name + ".use_battery_status", use_battery_status_);
   node_->get_parameter(name + ".use_external_detection_pose", use_external_detection_pose_);
   node_->get_parameter(name + ".external_detection_timeout", external_detection_timeout_);
@@ -98,6 +103,13 @@ void SimpleChargingDock::configure(
   node_->get_parameter(name + ".staging_x_offset", staging_x_offset_);
   node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_);
 
+  std::string dock_direction;
+  node_->get_parameter(name + ".dock_direction", dock_direction);
+  dock_direction_ = utils::getDockDirectionFromString(dock_direction);
+  if (dock_direction_ == opennav_docking_core::DockDirection::UNKNOWN) {
+    throw std::runtime_error{"Dock direction is not valid. Valid options are: forward or backward"};
+  }
+
   // Setup filter
   double filter_coef;
   node_->get_parameter(name + ".filter_coef", filter_coef);
diff --git a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp
index 25e67c5eb94..54001bfe1b6 100644
--- a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp
+++ b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp
@@ -16,6 +16,7 @@
 
 #include "nav2_util/node_utils.hpp"
 #include "opennav_docking/simple_non_charging_dock.hpp"
+#include "opennav_docking/utils.hpp"
 
 namespace opennav_docking
 {
@@ -69,6 +70,10 @@ void SimpleNonChargingDock::configure(
   nav2_util::declare_parameter_if_not_declared(
     node_, name + ".staging_yaw_offset", rclcpp::ParameterValue(0.0));
 
+  // Direction of docking
+  nav2_util::declare_parameter_if_not_declared(
+    node_, name + ".dock_direction", rclcpp::ParameterValue(std::string("forward")));
+
   node_->get_parameter(name + ".use_external_detection_pose", use_external_detection_pose_);
   node_->get_parameter(name + ".external_detection_timeout", external_detection_timeout_);
   node_->get_parameter(
@@ -87,6 +92,13 @@ void SimpleNonChargingDock::configure(
   node_->get_parameter(name + ".staging_x_offset", staging_x_offset_);
   node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_);
 
+  std::string dock_direction;
+  node_->get_parameter(name + ".dock_direction", dock_direction);
+  dock_direction_ = utils::getDockDirectionFromString(dock_direction);
+  if (dock_direction_ == opennav_docking_core::DockDirection::UNKNOWN) {
+    throw std::runtime_error{"Dock direction is not valid. Valid options are: forward or backward"};
+  }
+
   // Setup filter
   double filter_coef;
   node_->get_parameter(name + ".filter_coef", filter_coef);
diff --git a/nav2_docking/opennav_docking/test/CMakeLists.txt b/nav2_docking/opennav_docking/test/CMakeLists.txt
index 65d6e1d68ab..d0ec76332c0 100644
--- a/nav2_docking/opennav_docking/test/CMakeLists.txt
+++ b/nav2_docking/opennav_docking/test/CMakeLists.txt
@@ -111,4 +111,20 @@ target_link_libraries(test_docking_server_unit
 )
 
 # Test docking server (smoke)
-ament_add_pytest_test("test_docking_server" test_docking_server.py)
+ament_add_pytest_test(test_docking_server_with_charging_dock test_docking_server.py
+  ENV
+  NON_CHARGING_DOCK=False
+  BACKWARD=False
+)
+
+ament_add_pytest_test(test_docking_server_with_non_charging_dock test_docking_server.py
+  ENV
+  NON_CHARGING_DOCK=True
+  BACKWARD=False
+)
+
+ament_add_pytest_test(test_docking_server_backward test_docking_server.py
+  ENV
+  NON_CHARGING_DOCK=False
+  BACKWARD=True
+)
diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_conversion_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_conversion_file.yaml
new file mode 100644
index 00000000000..99efe11c30d
--- /dev/null
+++ b/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_conversion_file.yaml
@@ -0,0 +1,5 @@
+docks:
+  dock1:
+    type: "dockv3"
+    frame: "map"
+    pose: [0.3, map, 0.0]
diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_pose_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_pose_file.yaml
new file mode 100644
index 00000000000..de412e00121
--- /dev/null
+++ b/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_pose_file.yaml
@@ -0,0 +1,5 @@
+docks:
+  dock1:
+    type: "dockv3"
+    frame: "mapA"
+    pose: [0.3, 0.3]
diff --git a/nav2_docking/opennav_docking/test/test_dock_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_file.yaml
similarity index 100%
rename from nav2_docking/opennav_docking/test/test_dock_file.yaml
rename to nav2_docking/opennav_docking/test/dock_files/test_dock_file.yaml
diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_no_pose_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_no_pose_file.yaml
new file mode 100644
index 00000000000..4d88a4da695
--- /dev/null
+++ b/nav2_docking/opennav_docking/test/dock_files/test_dock_no_pose_file.yaml
@@ -0,0 +1,4 @@
+docks:
+  dock1:
+    type: "dockv3"
+    frame: "mapA"
diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_no_type_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_no_type_file.yaml
new file mode 100644
index 00000000000..197e691c7af
--- /dev/null
+++ b/nav2_docking/opennav_docking/test/dock_files/test_dock_no_type_file.yaml
@@ -0,0 +1,4 @@
+docks:
+  dock1:
+    frame: "mapA"
+    pose: [0.3, 0.3, 0.0]
diff --git a/nav2_docking/opennav_docking/test/dock_files/test_no_docks_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_no_docks_file.yaml
new file mode 100644
index 00000000000..49195f2cb4c
--- /dev/null
+++ b/nav2_docking/opennav_docking/test/dock_files/test_no_docks_file.yaml
@@ -0,0 +1,5 @@
+no_docks:
+  dock1:
+    type: "dockv3"
+    frame: "mapA"
+    pose: [0.3, 0.3, 0.0]
diff --git a/nav2_docking/opennav_docking/test/docking_params.yaml b/nav2_docking/opennav_docking/test/docking_params.yaml
new file mode 100644
index 00000000000..79bc784b292
--- /dev/null
+++ b/nav2_docking/opennav_docking/test/docking_params.yaml
@@ -0,0 +1,18 @@
+docking_server:
+  ros__parameters:
+    wait_charge_timeout: 1.0
+    controller:
+      use_collision_detection: False
+      transform_tolerance: 0.5
+    dock_plugins: [test_dock_plugin]
+    test_dock_plugin:
+      plugin: opennav_docking::SimpleChargingDock
+      use_battery_status: True
+      dock_direction: forward
+      staging_yaw_offset: 0.0
+    docks: [test_dock]
+    test_dock:
+      type: test_dock_plugin
+      frame: odom
+      pose: [10.0, 0.0, 0.0]
+      tolerance: 0.5
diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp
index cae362d4f09..9d4f0b9f5ea 100644
--- a/nav2_docking/opennav_docking/test/test_controller.cpp
+++ b/nav2_docking/opennav_docking/test/test_controller.cpp
@@ -540,7 +540,7 @@ TEST(ControllerTests, CollisionCheckerUndockForward) {
 
 }  // namespace opennav_docking
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_docking/opennav_docking/test/test_dock_database.cpp b/nav2_docking/opennav_docking/test/test_dock_database.cpp
index 17f1ff11cbb..3b9322134ce 100644
--- a/nav2_docking/opennav_docking/test/test_dock_database.cpp
+++ b/nav2_docking/opennav_docking/test/test_dock_database.cpp
@@ -91,6 +91,48 @@ TEST(DatabaseTests, findTests)
   db.findDockPlugin("");
 }
 
+TEST(DatabaseTests, getDockInstancesBadConversionFile)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  std::vector<std::string> plugins{"dockv1"};
+  node->declare_parameter("dock_plugins", rclcpp::ParameterValue(plugins));
+  node->declare_parameter(
+    "dockv1.plugin",
+    rclcpp::ParameterValue("opennav_docking::SimpleChargingDock"));
+
+  // Set a valid path with a malformed file
+  node->declare_parameter(
+    "dock_database",
+    rclcpp::ParameterValue(
+      ament_index_cpp::get_package_share_directory("opennav_docking") +
+      "/dock_files/test_dock_bad_conversion_file.yaml"));
+
+  opennav_docking::DockDatabase db;
+  db.initialize(node, nullptr);
+
+  EXPECT_EQ(db.plugin_size(), 1u);
+  EXPECT_EQ(db.instance_size(), 0u);
+}
+
+TEST(DatabaseTests, getDockInstancesWrongPath)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  std::vector<std::string> plugins{"dockv1"};
+  node->declare_parameter("dock_plugins", rclcpp::ParameterValue(plugins));
+  node->declare_parameter(
+    "dockv1.plugin",
+    rclcpp::ParameterValue("opennav_docking::SimpleChargingDock"));
+
+  // Set a wrong path
+  node->declare_parameter("dock_database", rclcpp::ParameterValue("file_does_not_exist.yaml"));
+
+  opennav_docking::DockDatabase db;
+  db.initialize(node, nullptr);
+
+  EXPECT_EQ(db.plugin_size(), 1u);
+  EXPECT_EQ(db.instance_size(), 0u);
+}
+
 TEST(DatabaseTests, reloadDbService)
 {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
@@ -108,7 +150,7 @@ TEST(DatabaseTests, reloadDbService)
 
   auto request = std::make_shared<nav2_msgs::srv::ReloadDockDatabase::Request>();
   request->filepath = ament_index_cpp::get_package_share_directory("opennav_docking") +
-    "/test_dock_file.yaml";
+    "/dock_files/test_dock_file.yaml";
   EXPECT_TRUE(client->wait_for_service(1s));
   auto result = client->async_send_request(request);
   EXPECT_EQ(
@@ -128,9 +170,41 @@ TEST(DatabaseTests, reloadDbService)
   EXPECT_FALSE(result2.get()->success);
 }
 
+TEST(DatabaseTests, reloadDbMutexLocked)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  std::vector<std::string> plugins{"dockv1"};
+  node->declare_parameter("dock_plugins", rclcpp::ParameterValue(plugins));
+  node->declare_parameter(
+    "dockv1.plugin",
+    rclcpp::ParameterValue("opennav_docking::SimpleChargingDock"));
+
+  // This mutex is locked when dock / undock is called
+  auto mutex = std::make_shared<std::mutex>();
+  mutex->lock();
+  opennav_docking::DockDatabase db(mutex);
+  db.initialize(node, nullptr);
+
+  // Call service with a filepath
+  auto client =
+    node->create_client<nav2_msgs::srv::ReloadDockDatabase>("test/reload_database");
+
+  auto request = std::make_shared<nav2_msgs::srv::ReloadDockDatabase::Request>();
+  request->filepath = ament_index_cpp::get_package_share_directory("opennav_docking") +
+    "/dock_files/test_dock_file.yaml";
+  EXPECT_TRUE(client->wait_for_service(1s));
+  auto result = client->async_send_request(request);
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(node, result, 2s),
+    rclcpp::FutureReturnCode::SUCCESS);
+  EXPECT_FALSE(result.get()->success);
+
+  mutex->unlock();
+}
+
 }  // namespace opennav_docking
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py
index f402f7970e3..06e2fa1d105 100644
--- a/nav2_docking/opennav_docking/test/test_docking_server.py
+++ b/nav2_docking/opennav_docking/test/test_docking_server.py
@@ -13,6 +13,7 @@
 # limitations under the License.
 
 from math import acos, cos, sin
+import os
 import time
 import unittest
 
@@ -20,21 +21,22 @@
 from geometry_msgs.msg import TransformStamped, Twist, TwistStamped
 from launch import LaunchDescription
 # from launch.actions import SetEnvironmentVariable
+from launch.launch_context import LaunchContext
 from launch_ros.actions import Node
 import launch_testing
 import launch_testing.actions
 import launch_testing.asserts
 import launch_testing.markers
 import launch_testing.util
+from nav2_common.launch import RewrittenYaml
 from nav2_msgs.action import DockRobot, NavigateToPose, UndockRobot
 import pytest
 import rclpy
 from rclpy.action.client import ActionClient
-from rclpy.action.server import ActionServer
+from rclpy.action.server import ActionServer, ServerGoalHandle
 from sensor_msgs.msg import BatteryState
 from tf2_ros import TransformBroadcaster
 
-
 # This test can be run standalone with:
 # python3 -u -m pytest test_docking_server.py -s
 
@@ -42,10 +44,36 @@
 # try to identify flaky ness.
 # python3 -u -m pytest --force-flaky --min-passes 3 --max-runs 5 -s -v test_docking_server.py
 
+
 @pytest.mark.rostest
 # @pytest.mark.flaky
 # @pytest.mark.flaky(max_runs=5, min_passes=3)
-def generate_test_description():
+def generate_test_description() -> LaunchDescription:
+
+    # Use local param file
+    launch_dir = os.path.dirname(os.path.realpath(__file__))
+    params_file = os.path.join(launch_dir, 'docking_params.yaml')
+
+    # Replace the default parameter values for testing special features
+    # without having multiple params_files inside the nav2 stack
+    context = LaunchContext()
+    param_substitutions = {}
+
+    if os.getenv('NON_CHARGING_DOCK') == 'True':
+        param_substitutions.update({'plugin': 'opennav_docking::SimpleNonChargingDock'})
+
+    if os.getenv('BACKWARD') == 'True':
+        param_substitutions.update({'dock_direction': 'backward'})
+        param_substitutions.update({'staging_yaw_offset': '3.14'})
+
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites=param_substitutions,
+        convert_types=True,
+    )
+
+    new_yaml = configured_params.perform(context)
 
     return LaunchDescription([
         # SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
@@ -54,21 +82,7 @@ def generate_test_description():
             package='opennav_docking',
             executable='opennav_docking',
             name='docking_server',
-            parameters=[{'wait_charge_timeout': 1.0,
-                         'controller': {
-                             'use_collision_detection': False,
-                             'transform_tolerance': 0.5,
-                         },
-                         'dock_plugins': ['test_dock_plugin'],
-                         'test_dock_plugin': {
-                             'plugin': 'opennav_docking::SimpleChargingDock',
-                             'use_battery_status': True},
-                         'docks': ['test_dock'],
-                         'test_dock': {
-                             'type': 'test_dock_plugin',
-                             'frame': 'odom',
-                             'pose': [10.0, 0.0, 0.0]
-                         }}],
+            parameters=[new_yaml],
             output='screen',
         ),
         Node(
@@ -86,33 +100,36 @@ def generate_test_description():
 class TestDockingServer(unittest.TestCase):
 
     @classmethod
-    def setUpClass(cls):
+    def setUpClass(cls) -> None:
         rclpy.init()
 
     @classmethod
-    def tearDownClass(cls):
+    def tearDownClass(cls) -> None:
         rclpy.shutdown()
 
-    def setUp(self):
+    def setUp(self) -> None:
         # Create a ROS node for tests
         # Latest odom -> base_link
         self.x = 0.0
         self.y = 0.0
         self.theta = 0.0
+        # If BACKWARD is set, start facing backward
+        if os.getenv('BACKWARD') == 'True':
+            self.theta = 3.14
         # Track charge state
         self.is_charging = False
         # Latest command velocity
         self.command = Twist()
         self.node = rclpy.create_node('test_docking_server')
 
-    def tearDown(self):
+    def tearDown(self) -> None:
         self.node.destroy_node()
 
-    def command_velocity_callback(self, msg):
-        self.node.get_logger().info('Command: %f %f' % (msg.twist.linear.x, msg.twist.angular.z))
+    def command_velocity_callback(self, msg: TwistStamped) -> None:
+        self.node.get_logger().info(f'Command: {msg.twist.linear.x:f} {msg.twist.angular.z:f}')
         self.command = msg.twist
 
-    def timer_callback(self):
+    def timer_callback(self) -> None:
         # Propagate command
         period = 0.05
         self.x += cos(self.theta) * self.command.linear.x * period
@@ -121,7 +138,7 @@ def timer_callback(self):
         # Need to publish updated TF
         self.publish()
 
-    def publish(self):
+    def publish(self) -> None:
         # Publish base->odom transform
         t = TransformStamped()
         t.header.stamp = self.node.get_clock().now().to_msg()
@@ -132,15 +149,16 @@ def publish(self):
         t.transform.rotation.z = sin(self.theta / 2.0)
         t.transform.rotation.w = cos(self.theta / 2.0)
         self.tf_broadcaster.sendTransform(t)
-        # Publish battery state
-        b = BatteryState()
-        if self.is_charging:
-            b.current = 1.0
-        else:
-            b.current = -1.0
-        self.battery_state_pub.publish(b)
-
-    def action_feedback_callback(self, msg):
+        # Publish the battery state if we are using a charging dock
+        if os.getenv('NON_CHARGING_DOCK') == 'False':
+            b = BatteryState()
+            if self.is_charging:
+                b.current = 1.0
+            else:
+                b.current = -1.0
+            self.battery_state_pub.publish(b)
+
+    def action_feedback_callback(self, msg: DockRobot.Feedback) -> None:
         # Force the docking action to run a full recovery loop and then
         # make contact with the dock (based on pose of robot) before
         # we report that the robot is charging
@@ -148,12 +166,16 @@ def action_feedback_callback(self, msg):
                 msg.feedback.state == msg.feedback.WAIT_FOR_CHARGE:
             self.is_charging = True
 
-    def nav_execute_callback(self, goal_handle):
+    def nav_execute_callback(
+        self,
+        goal_handle: ServerGoalHandle[NavigateToPose.Goal,
+                                      NavigateToPose.Result, NavigateToPose.Feedback]
+    ) -> NavigateToPose.Result:
         goal = goal_handle.request
         self.x = goal.pose.pose.position.x - 0.05
         self.y = goal.pose.pose.position.y + 0.05
         self.theta = 2.0 * acos(goal.pose.pose.orientation.w)
-        self.node.get_logger().info('Navigating to %f %f %f' % (self.x, self.y, self.theta))
+        self.node.get_logger().info(f'Navigating to {self.x:f} {self.y:f} {self.theta:f}')
         goal_handle.succeed()
         self.publish()
 
@@ -162,7 +184,7 @@ def nav_execute_callback(self, goal_handle):
         result.error_msg = ''
         return result
 
-    def test_docking_server(self):
+    def test_docking_server(self) -> None:
         # Publish TF for odometry
         self.tf_broadcaster = TransformBroadcaster(self.node)
 
@@ -181,12 +203,13 @@ def test_docking_server(self):
             10
         )
 
-        # Create publisher for battery state message
-        self.battery_state_pub = self.node.create_publisher(
-            BatteryState,
-            'battery_state',
-            10
-        )
+        # Create publisher for battery state message if we are using a charging dock
+        if os.getenv('NON_CHARGING_DOCK') == 'False':
+            self.battery_state_pub = self.node.create_publisher(
+                BatteryState,
+                'battery_state',
+                10
+            )
 
         # Mock out navigation server (just teleport the robot)
         self.action_server = ActionServer(
@@ -206,7 +229,7 @@ def test_docking_server(self):
         # Test docking action
         self.action_result = []
         assert self.dock_action_client.wait_for_server(timeout_sec=5.0), \
-               'dock_robot service not available'
+            'dock_robot service not available'
 
         goal = DockRobot.Goal()
         goal.use_dock_id = True
@@ -239,8 +262,10 @@ def test_docking_server(self):
         self.action_result.append(result_future_original.result())
 
         # First is aborted due to preemption
-        self.assertEqual(self.action_result[0].status, GoalStatus.STATUS_ABORTED)
-        self.assertFalse(self.action_result[0].result.success)
+        self.assertIsNotNone(self.action_result[0])
+        if self.action_result[0] is not None:
+            self.assertEqual(self.action_result[0].status, GoalStatus.STATUS_ABORTED)
+            self.assertFalse(self.action_result[0].result.success)
 
         self.node.get_logger().info('Goal preempted')
 
@@ -250,8 +275,10 @@ def test_docking_server(self):
             time.sleep(0.1)
 
         # Second is aborted due to preemption during main loop (takes down all actions)
-        self.assertEqual(self.action_result[1].status, GoalStatus.STATUS_ABORTED)
-        self.assertFalse(self.action_result[1].result.success)
+        self.assertIsNotNone(self.action_result[1])
+        if self.action_result[1] is not None:
+            self.assertEqual(self.action_result[1].status, GoalStatus.STATUS_ABORTED)
+            self.assertFalse(self.action_result[1].result.success)
 
         # Resend the goal
         self.node.get_logger().info('Sending goal again')
@@ -265,9 +292,14 @@ def test_docking_server(self):
         rclpy.spin_until_future_complete(self.node, result_future)
         self.action_result.append(result_future.result())
 
-        self.assertEqual(self.action_result[2].status, GoalStatus.STATUS_SUCCEEDED)
-        self.assertTrue(self.action_result[2].result.success)
-        self.assertEqual(self.action_result[2].result.num_retries, 1)
+        self.assertIsNotNone(self.action_result[2])
+        if self.action_result[2] is not None:
+            self.assertEqual(self.action_result[2].status, GoalStatus.STATUS_SUCCEEDED)
+            self.assertTrue(self.action_result[2].result.success)
+            if os.getenv('NON_CHARGING_DOCK') == 'False':
+                self.assertEqual(self.action_result[2].result.num_retries, 1)
+            else:
+                self.assertEqual(self.action_result[2].result.num_retries, 0)
 
         # Test undocking action
         self.is_charging = False
@@ -283,13 +315,15 @@ def test_docking_server(self):
         rclpy.spin_until_future_complete(self.node, result_future)
         self.action_result.append(result_future.result())
 
-        self.assertEqual(self.action_result[3].status, GoalStatus.STATUS_SUCCEEDED)
-        self.assertTrue(self.action_result[3].result.success)
+        self.assertIsNotNone(self.action_result[3])
+        if self.action_result[3] is not None:
+            self.assertEqual(self.action_result[3].status, GoalStatus.STATUS_SUCCEEDED)
+            self.assertTrue(self.action_result[3].result.success)
 
 
 @launch_testing.post_shutdown_test()
 class TestProcessOutput(unittest.TestCase):
 
-    def test_exit_code(self, proc_info):
+    def test_exit_code(self, proc_info: launch_testing.ProcInfoHandler) -> None:
         # Check that all processes in the launch exit with code 0
         launch_testing.asserts.assertExitCodes(proc_info)
diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp
index dd7d08d39f1..a97448b2d7d 100644
--- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp
+++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp
@@ -39,6 +39,11 @@ class DockingServerShim : public DockingServer
   {
     return geometry_msgs::msg::PoseStamped();
   }
+
+  std::optional<bool> getDockBackward()
+  {
+    return dock_backwards_;
+  }
 };
 
 TEST(DockingServerTests, ObjectLifecycle)
@@ -263,9 +268,53 @@ TEST(DockingServerTests, testDynamicParams)
   node.reset();
 }
 
+TEST(DockingServerTests, testDockBackward)
+{
+  auto node = std::make_shared<DockingServerShim>();
+
+  // Setup 1 instance of the test failure dock & its plugin instance
+  node->declare_parameter(
+    "docks",
+    rclcpp::ParameterValue(std::vector<std::string>{"test_dock"}));
+  node->declare_parameter(
+    "test_dock.type",
+    rclcpp::ParameterValue(std::string{"dock_plugin"}));
+  node->declare_parameter(
+    "test_dock.pose",
+    rclcpp::ParameterValue(std::vector<double>{0.0, 0.0, 0.0}));
+  node->declare_parameter(
+    "dock_plugins",
+    rclcpp::ParameterValue(std::vector<std::string>{"dock_plugin"}));
+  node->declare_parameter(
+    "dock_plugin.plugin",
+    rclcpp::ParameterValue(std::string{"opennav_docking::TestFailureDock"}));
+
+  // The dock_backwards parameter should be declared but not set
+  node->on_configure(rclcpp_lifecycle::State());
+  EXPECT_FALSE(node->getDockBackward().has_value());
+  node->on_cleanup(rclcpp_lifecycle::State());
+
+  // Now, set the dock_backwards parameter to true
+  node->set_parameter(rclcpp::Parameter("dock_backwards", rclcpp::ParameterValue(true)));
+  node->on_configure(rclcpp_lifecycle::State());
+  EXPECT_TRUE(node->getDockBackward().has_value());
+  EXPECT_TRUE(node->getDockBackward().value());
+  node->on_cleanup(rclcpp_lifecycle::State());
+
+  // Now, set the dock_backwards parameter to false
+  node->set_parameter(rclcpp::Parameter("dock_backwards", rclcpp::ParameterValue(false)));
+  node->on_configure(rclcpp_lifecycle::State());
+  EXPECT_TRUE(node->getDockBackward().has_value());
+  EXPECT_FALSE(node->getDockBackward().value());
+  node->on_cleanup(rclcpp_lifecycle::State());
+
+  node->on_shutdown(rclcpp_lifecycle::State());
+  node.reset();
+}
+
 }  // namespace opennav_docking
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_docking/opennav_docking/test/test_navigator.cpp b/nav2_docking/opennav_docking/test/test_navigator.cpp
index 74df97e62d0..f3cd3ad3308 100644
--- a/nav2_docking/opennav_docking/test/test_navigator.cpp
+++ b/nav2_docking/opennav_docking/test/test_navigator.cpp
@@ -81,6 +81,28 @@ class DummyNavigationServer : rclcpp::Node
   bool toggle_{false};
 };
 
+TEST(NavigatorTests, TestNavigatorReconfigure)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test_node");
+  auto navigator = std::make_unique<Navigator>(node);
+  node->configure();
+  node->activate();
+  navigator->activate();
+  navigator->deactivate();
+  navigator.reset();
+
+  // Create and activate again
+  EXPECT_NO_THROW(navigator = std::make_unique<Navigator>(node));
+  navigator->activate();
+  navigator->deactivate();
+
+  // Reset the node
+  navigator.reset();
+  node->deactivate();
+  node->cleanup();
+  node->shutdown();
+}
+
 TEST(NavigatorTests, TestNavigator)
 {
   auto dummy_navigator_node = std::make_shared<DummyNavigationServer>();
@@ -94,32 +116,37 @@ TEST(NavigatorTests, TestNavigator)
   // Should succeed, action server set to succeed
   dummy_navigator_node->setReturn(true);
   EXPECT_NO_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
+    navigator->goToPose(
+      geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
       is_preempted_false));
 
   // Should fail, timeout exceeded
   EXPECT_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(0.0, 0.0),
+    navigator->goToPose(
+      geometry_msgs::msg::PoseStamped(), rclcpp::Duration(0.0, 0.0),
       is_preempted_false),
     opennav_docking_core::FailedToStage);
 
   // Should fail, action server set to succeed
   dummy_navigator_node->setReturn(false);
   EXPECT_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
+    navigator->goToPose(
+      geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
       is_preempted_false),
     opennav_docking_core::FailedToStage);
 
   // First should fail, recursion should succeed
   dummy_navigator_node->setToggle();
   EXPECT_NO_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
+    navigator->goToPose(
+      geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
       is_preempted_false));
 
   // Should fail, preempted
   dummy_navigator_node->setReturn(true);
   EXPECT_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
+    navigator->goToPose(
+      geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
       is_preempted_true),
     opennav_docking_core::FailedToStage);
 
@@ -130,7 +157,7 @@ TEST(NavigatorTests, TestNavigator)
 
 }  // namespace opennav_docking
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_docking/opennav_docking/test/test_pose_filter.cpp b/nav2_docking/opennav_docking/test/test_pose_filter.cpp
index 207fff3f8c9..6d9ec8817fa 100644
--- a/nav2_docking/opennav_docking/test/test_pose_filter.cpp
+++ b/nav2_docking/opennav_docking/test/test_pose_filter.cpp
@@ -84,6 +84,16 @@ TEST(PoseFilterTests, FilterTests)
   EXPECT_NEAR(pose.pose.position.x, 2.0, 0.0001);
   EXPECT_NEAR(pose.pose.position.y, 4.0, 0.0001);
   EXPECT_NEAR(pose.pose.position.z, 6.0, 0.0001);
+
+  // Create a non-filtering pose filter
+  PoseFilter no_filter(0.0, 1.0);
+  // Update with the first measurement
+  pose = no_filter.update(meas1);
+  // Check that the pose is the same as the measurement
+  EXPECT_NEAR(pose.pose.position.x, 1.0, 0.0001);
+  EXPECT_NEAR(pose.pose.position.y, 3.0, 0.0001);
+  EXPECT_NEAR(pose.pose.position.z, 5.0, 0.0001);
+  EXPECT_EQ(pose.pose.orientation.w, 1.0);
 }
 
 }  // namespace opennav_docking
diff --git a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp
index 09d8bab0e1a..ccd4825863b 100644
--- a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp
+++ b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp
@@ -27,6 +27,18 @@
 namespace opennav_docking
 {
 
+class SimpleChargingDockShim : public opennav_docking::SimpleChargingDock
+{
+public:
+  SimpleChargingDockShim()
+  : opennav_docking::SimpleChargingDock() {}
+
+  std::vector<std::string> getStallJointNames()
+  {
+    return stall_joint_names_;
+  }
+};
+
 TEST(SimpleChargingDockTests, ObjectLifecycle)
 {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
@@ -96,15 +108,23 @@ TEST(SimpleChargingDockTests, StallDetection)
     "joint_states", rclcpp::QoS(1));
   pub->on_activate();
   node->declare_parameter("my_dock.use_stall_detection", rclcpp::ParameterValue(true));
-  std::vector<std::string> names = {"left_motor", "right_motor"};
-  node->declare_parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names));
+  node->declare_parameter("my_dock.stall_joint_names", rclcpp::PARAMETER_STRING_ARRAY);
   node->declare_parameter("my_dock.stall_velocity_threshold", rclcpp::ParameterValue(0.1));
   node->declare_parameter("my_dock.stall_effort_threshold", rclcpp::ParameterValue(5.0));
 
-  auto dock = std::make_unique<opennav_docking::SimpleChargingDock>();
+  auto dock = std::make_unique<opennav_docking::SimpleChargingDockShim>();
+  dock->configure(node, "my_dock", nullptr);
+  // Check that the joint names are empty, showing the error
+  EXPECT_TRUE(dock->getStallJointNames().empty());
+  dock->cleanup();
 
+  // Now set the joint names
+  std::vector<std::string> names = {"left_motor", "right_motor"};
+  node->set_parameter(
+    rclcpp::Parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names)));
   dock->configure(node, "my_dock", nullptr);
   dock->activate();
+  EXPECT_EQ(dock->getStallJointNames(), names);
 
   // Stopped, but below effort threshold
   sensor_msgs::msg::JointState msg;
@@ -234,9 +254,118 @@ TEST(SimpleChargingDockTests, RefinedPoseTest)
   dock.reset();
 }
 
+TEST(SimpleChargingDockTests, RefinedPoseNotTransform)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true));
+  auto pub = node->create_publisher<geometry_msgs::msg::PoseStamped>(
+    "detected_dock_pose", rclcpp::QoS(1));
+  pub->on_activate();
+  auto dock = std::make_unique<opennav_docking::SimpleChargingDock>();
+
+  // Create the TF
+  auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  tf_buffer->setUsingDedicatedThread(true);
+
+  dock->configure(node, "my_dock", tf_buffer);
+  dock->activate();
+
+  geometry_msgs::msg::PoseStamped detected_pose;
+  detected_pose.header.stamp = node->now();
+  detected_pose.header.frame_id = "my_frame";
+  detected_pose.pose.position.x = 1.0;
+  detected_pose.pose.position.y = 1.0;
+  pub->publish(detected_pose);
+  rclcpp::spin_some(node->get_node_base_interface());
+
+  // Create a pose with a different frame_id
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header.frame_id = "other_frame";
+
+  // It can not find a transform between the two frames
+  EXPECT_FALSE(dock->getRefinedPose(pose, ""));
+
+  dock->deactivate();
+  dock->cleanup();
+  dock.reset();
+  tf_buffer.reset();
+}
+
+TEST(SimpleChargingDockTests, IsDockedTransformException)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true));
+  auto pub = node->create_publisher<geometry_msgs::msg::PoseStamped>(
+    "detected_dock_pose", rclcpp::QoS(1));
+  pub->on_activate();
+  auto dock = std::make_unique<opennav_docking::SimpleChargingDock>();
+
+  // Create the TF
+  auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  tf_buffer->setUsingDedicatedThread(true);
+
+  dock->configure(node, "my_dock", tf_buffer);
+  dock->activate();
+
+  geometry_msgs::msg::PoseStamped detected_pose;
+  detected_pose.header.stamp = node->now();
+  detected_pose.header.frame_id = "my_frame";
+  detected_pose.pose.position.x = 1.0;
+  detected_pose.pose.position.y = 1.0;
+  pub->publish(detected_pose);
+  rclcpp::spin_some(node->get_node_base_interface());
+
+  // Create a pose with a different frame_id
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header.frame_id = "other_frame";
+
+  // Set a transform between the two frames
+  geometry_msgs::msg::TransformStamped transform;
+  transform.header.stamp = node->now();
+  transform.header.frame_id = "my_frame";
+  transform.child_frame_id = "other_frame";
+  tf_buffer->setTransform(transform, "test", true);
+
+  // It can find a transform between the two frames but it throws an exception in isDocked
+  EXPECT_TRUE(dock->getRefinedPose(pose, ""));
+  EXPECT_FALSE(dock->isDocked());
+
+  dock->deactivate();
+  dock->cleanup();
+  dock.reset();
+}
+
+TEST(SimpleChargingDockTests, GetDockDirection)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  node->declare_parameter("my_dock.dock_direction", rclcpp::ParameterValue("forward"));
+
+  auto dock = std::make_unique<opennav_docking::SimpleChargingDock>();
+  EXPECT_EQ(dock->getDockDirection(), opennav_docking_core::DockDirection::UNKNOWN);
+  EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr));
+  EXPECT_EQ(dock->getDockDirection(), opennav_docking_core::DockDirection::FORWARD);
+  dock->cleanup();
+
+  // Now set to BACKWARD
+  node->set_parameter(
+    rclcpp::Parameter("my_dock.dock_direction", rclcpp::ParameterValue("backward")));
+  EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr));
+  EXPECT_EQ(dock->getDockDirection(), opennav_docking_core::DockDirection::BACKWARD);
+  dock->cleanup();
+
+  // Now set to UNKNOWN
+  node->set_parameter(
+    rclcpp::Parameter("my_dock.dock_direction", rclcpp::ParameterValue("other")));
+  EXPECT_THROW(dock->configure(node, "my_dock", nullptr), std::runtime_error);
+  EXPECT_EQ(dock->getDockDirection(), opennav_docking_core::DockDirection::UNKNOWN);
+
+  dock->cleanup();
+  dock.reset();
+}
+
 }  // namespace opennav_docking
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp
index 5d49b91bea6..68bfc193379 100644
--- a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp
+++ b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp
@@ -27,6 +27,18 @@
 namespace opennav_docking
 {
 
+class SimpleNonChargingDockShim : public opennav_docking::SimpleNonChargingDock
+{
+public:
+  SimpleNonChargingDockShim()
+  : opennav_docking::SimpleNonChargingDock() {}
+
+  std::vector<std::string> getStallJointNames()
+  {
+    return stall_joint_names_;
+  }
+};
+
 TEST(SimpleNonChargingDockTests, ObjectLifecycle)
 {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
@@ -54,14 +66,23 @@ TEST(SimpleNonChargingDockTests, StallDetection)
     "joint_states", rclcpp::QoS(1));
   pub->on_activate();
   node->declare_parameter("my_dock.use_stall_detection", rclcpp::ParameterValue(true));
-  std::vector<std::string> names = {"left_motor", "right_motor"};
-  node->declare_parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names));
+  node->declare_parameter("my_dock.stall_joint_names", rclcpp::PARAMETER_STRING_ARRAY);
   node->declare_parameter("my_dock.stall_velocity_threshold", rclcpp::ParameterValue(0.1));
   node->declare_parameter("my_dock.stall_effort_threshold", rclcpp::ParameterValue(5.0));
 
-  auto dock = std::make_unique<opennav_docking::SimpleNonChargingDock>();
+  auto dock = std::make_unique<opennav_docking::SimpleNonChargingDockShim>();
+  dock->configure(node, "my_dock", nullptr);
+  // Check that the joint names are empty, showing the error
+  EXPECT_TRUE(dock->getStallJointNames().empty());
+  dock->cleanup();
 
+  // Now set the joint names
+  std::vector<std::string> names = {"left_motor", "right_motor"};
+  node->set_parameter(
+    rclcpp::Parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names)));
   dock->configure(node, "my_dock", nullptr);
+  EXPECT_EQ(dock->getStallJointNames(), names);
+
   dock->activate();
   geometry_msgs::msg::PoseStamped pose;
   EXPECT_TRUE(dock->getRefinedPose(pose, ""));
@@ -194,9 +215,118 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest)
   dock.reset();
 }
 
+TEST(SimpleNonChargingDockTests, RefinedPoseNotTransform)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true));
+  auto pub = node->create_publisher<geometry_msgs::msg::PoseStamped>(
+    "detected_dock_pose", rclcpp::QoS(1));
+  pub->on_activate();
+  auto dock = std::make_unique<opennav_docking::SimpleNonChargingDock>();
+
+  // Create the TF
+  auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  tf_buffer->setUsingDedicatedThread(true);
+
+  dock->configure(node, "my_dock", tf_buffer);
+  dock->activate();
+
+  geometry_msgs::msg::PoseStamped detected_pose;
+  detected_pose.header.stamp = node->now();
+  detected_pose.header.frame_id = "my_frame";
+  detected_pose.pose.position.x = 1.0;
+  detected_pose.pose.position.y = 1.0;
+  pub->publish(detected_pose);
+  rclcpp::spin_some(node->get_node_base_interface());
+
+  // Create a pose with a different frame_id
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header.frame_id = "other_frame";
+
+  // It can not find a transform between the two frames
+  EXPECT_FALSE(dock->getRefinedPose(pose, ""));
+
+  dock->deactivate();
+  dock->cleanup();
+  dock.reset();
+  tf_buffer.reset();
+}
+
+TEST(SimpleNonChargingDockTests, IsDockedTransformException)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true));
+  auto pub = node->create_publisher<geometry_msgs::msg::PoseStamped>(
+    "detected_dock_pose", rclcpp::QoS(1));
+  pub->on_activate();
+  auto dock = std::make_unique<opennav_docking::SimpleNonChargingDock>();
+
+  // Create the TF
+  auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  tf_buffer->setUsingDedicatedThread(true);
+
+  dock->configure(node, "my_dock", tf_buffer);
+  dock->activate();
+
+  geometry_msgs::msg::PoseStamped detected_pose;
+  detected_pose.header.stamp = node->now();
+  detected_pose.header.frame_id = "my_frame";
+  detected_pose.pose.position.x = 1.0;
+  detected_pose.pose.position.y = 1.0;
+  pub->publish(detected_pose);
+  rclcpp::spin_some(node->get_node_base_interface());
+
+  // Create a pose with a different frame_id
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header.frame_id = "other_frame";
+
+  // Set a transform between the two frames
+  geometry_msgs::msg::TransformStamped transform;
+  transform.header.stamp = node->now();
+  transform.header.frame_id = "my_frame";
+  transform.child_frame_id = "other_frame";
+  tf_buffer->setTransform(transform, "test", true);
+
+  // It can find a transform between the two frames but it throws an exception in isDocked
+  EXPECT_TRUE(dock->getRefinedPose(pose, ""));
+  EXPECT_FALSE(dock->isDocked());
+
+  dock->deactivate();
+  dock->cleanup();
+  dock.reset();
+}
+
+TEST(SimpleNonChargingDockTests, GetDockDirection)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  node->declare_parameter("my_dock.dock_direction", rclcpp::ParameterValue("forward"));
+
+  auto dock = std::make_unique<opennav_docking::SimpleNonChargingDock>();
+  EXPECT_EQ(dock->getDockDirection(), opennav_docking_core::DockDirection::UNKNOWN);
+  EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr));
+  EXPECT_EQ(dock->getDockDirection(), opennav_docking_core::DockDirection::FORWARD);
+  dock->cleanup();
+
+  // Now set to BACKWARD
+  node->set_parameter(
+    rclcpp::Parameter("my_dock.dock_direction", rclcpp::ParameterValue("backward")));
+  EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr));
+  EXPECT_EQ(dock->getDockDirection(), opennav_docking_core::DockDirection::BACKWARD);
+  dock->cleanup();
+
+  // Now set to UNKNOWN
+  node->set_parameter(
+    rclcpp::Parameter("my_dock.dock_direction", rclcpp::ParameterValue("other")));
+  EXPECT_THROW(dock->configure(node, "my_dock", nullptr), std::runtime_error);
+  EXPECT_EQ(dock->getDockDirection(), opennav_docking_core::DockDirection::UNKNOWN);
+
+  dock->cleanup();
+  dock.reset();
+}
+
 }  // namespace opennav_docking
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_docking/opennav_docking/test/test_utils.cpp b/nav2_docking/opennav_docking/test/test_utils.cpp
index 46bd52313f7..f92da221d56 100644
--- a/nav2_docking/opennav_docking/test/test_utils.cpp
+++ b/nav2_docking/opennav_docking/test/test_utils.cpp
@@ -92,7 +92,7 @@ TEST(UtilsTests, parseDockFile)
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test4");
   DockMap db;
   std::string filepath = ament_index_cpp::get_package_share_directory("opennav_docking") +
-    "/test_dock_file.yaml";
+    "/dock_files/test_dock_file.yaml";
   EXPECT_TRUE(utils::parseDockFile(filepath, node, db));
   EXPECT_EQ(db.size(), 2u);
   EXPECT_EQ(db["dock1"].frame, std::string("mapA"));
@@ -109,6 +109,32 @@ TEST(UtilsTests, parseDockFile)
   EXPECT_EQ(db["dock2"].id, std::string("2"));
 }
 
+TEST(UtilsTests, parseDockFile2)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test4");
+  DockMap db;
+
+  // Test with a file that has no docks
+  std::string filepath = ament_index_cpp::get_package_share_directory("opennav_docking") +
+    "/dock_files/test_no_docks_file.yaml";
+  EXPECT_FALSE(utils::parseDockFile(filepath, node, db));
+
+  // Test with a file that has no type
+  filepath = ament_index_cpp::get_package_share_directory("opennav_docking") +
+    "/dock_files/test_dock_no_type_file.yaml";
+  EXPECT_FALSE(utils::parseDockFile(filepath, node, db));
+
+  // Test with a file that has no pose
+  filepath = ament_index_cpp::get_package_share_directory("opennav_docking") +
+    "/dock_files/test_dock_no_pose_file.yaml";
+  EXPECT_FALSE(utils::parseDockFile(filepath, node, db));
+
+  // Test with a file that has wring pose array size
+  filepath = ament_index_cpp::get_package_share_directory("opennav_docking") +
+    "/dock_files/test_dock_bad_pose_file.yaml";
+  EXPECT_FALSE(utils::parseDockFile(filepath, node, db));
+}
+
 TEST(UtilsTests, testgetDockPoseStamped)
 {
   Dock d;
@@ -135,9 +161,16 @@ TEST(UtilsTests, testl2Norm)
   EXPECT_NEAR(utils::l2Norm(a, b), 0.734, 1e-3);
 }
 
+TEST(UtilsTests, testGetDockDirectionFromString) {
+  using opennav_docking_core::DockDirection;
+  EXPECT_EQ(utils::getDockDirectionFromString("forward"), DockDirection::FORWARD);
+  EXPECT_EQ(utils::getDockDirectionFromString("backward"), DockDirection::BACKWARD);
+  EXPECT_EQ(utils::getDockDirectionFromString("other"), DockDirection::UNKNOWN);
+}
+
 }  // namespace opennav_docking
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_docking/opennav_docking/test/testing_dock.cpp b/nav2_docking/opennav_docking/test/testing_dock.cpp
index 96ea5ff9370..a225291d804 100644
--- a/nav2_docking/opennav_docking/test/testing_dock.cpp
+++ b/nav2_docking/opennav_docking/test/testing_dock.cpp
@@ -38,6 +38,7 @@ class TestFailureDock : public opennav_docking_core::ChargingDock
     if (!node_) {
       throw std::runtime_error{"Failed to lock node"};
     }
+    dock_direction_ = opennav_docking_core::DockDirection::FORWARD;
   }
 
   virtual void cleanup() {}
diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp
index 902323ae805..ada25c9f474 100644
--- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp
+++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp
@@ -26,6 +26,12 @@
 namespace opennav_docking_core
 {
 
+/**
+ * @enum DockDirection
+ * @brief An enum class representing the direction of the dock
+ */
+enum class DockDirection { UNKNOWN, FORWARD, BACKWARD };
+
 /**
  * @class ChargingDock
  * @brief Abstract interface for a charging dock for the docking framework
@@ -123,10 +129,18 @@ class ChargingDock
    */
   virtual bool isCharger() {return true;}
 
+  /**
+   * @brief Indicates the direction of the dock. This is used to determine if the
+   * robot should drive forwards or backwards onto the dock.
+   * @return DockDirection The direction of the dock
+   */
+  DockDirection getDockDirection() {return dock_direction_;}
+
   std::string getName() {return name_;}
 
 protected:
   std::string name_;
+  DockDirection dock_direction_{DockDirection::UNKNOWN};
 };
 
 }  // namespace opennav_docking_core
diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
index 64757f31691..4f168a7d8df 100644
--- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
+++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
@@ -514,7 +514,7 @@ DWBLocalPlanner::transformGlobalPlan(
   auto prune_point = nav2_util::geometry_utils::first_after_integrated_distance(
     global_plan_.poses.begin(), global_plan_.poses.end(), forward_prune_distance_);
 
-  // Find the first pose in the plan (upto prune_point) that's less than transform_start_threshold
+  // Find the first pose in the plan (up to prune_point) that's less than transform_start_threshold
   // from the robot.
   auto transformation_begin = std::find_if(
     begin(global_plan_.poses), prune_point,
diff --git a/nav2_graceful_controller/README.md b/nav2_graceful_controller/README.md
index 0154550682e..331a91352b1 100644
--- a/nav2_graceful_controller/README.md
+++ b/nav2_graceful_controller/README.md
@@ -13,30 +13,30 @@ The smooth control law is a pose-following kinematic control law that generates
 
 ## Parameters
 
-| Parameter | Description | 
+| Parameter | Description |
 |-----|----|
-| `transform_tolerance` | The TF transform tolerance. | 
+| `transform_tolerance` | The TF transform tolerance. |
 | `motion_target_dist` | The lookahead distance to use to find the motion_target point. This distance should be a value around 1.0m but not much farther away. Greater values will cause the robot to generate smoother paths but not necessarily follow the path as closely. |
-| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | 
-| `k_phi` | Ratio of the rate of change in phi to the rate of change in r. Controls the convergence of the slow subsystem. If this value is equal to zero, the controller will behave as a pure waypoint follower. A high value offers extreme scenario of pose-following where theta is reduced much faster than r. **Note**: This variable is called k1 in earlier versions of the paper. | 
-| `k_delta` | Constant factor applied to the heading error feedback. Controls the convergence of the fast subsystem. The bigger the value, the robot converge faster to the reference heading. **Note**: This variable is called k2 in earlier versions of the paper. | 
-| `beta` | Constant factor applied to the path curvature. This value must be positive. Determines how fast the velocity drops when the curvature increases. | 
-| `lambda` | Constant factor applied to the path curvature. This value must be greater or equal to 1. Determines the sharpness of the curve: higher lambda implies sharper curves. | 
-| `v_linear_min` | Minimum linear velocity. Units: meters/sec. | 
-| `v_linear_max` | Maximum linear velocity. Units: meters/sec. | 
-| `v_angular_max` | Maximum angular velocity produced by the control law. Units: radians/sec. | 
-| `slowdown_radius` | Radius around the goal pose in which the robot will start to slow down. Units: meters. | 
-| `initial_rotation` | Enable a rotation in place to the goal before starting the path. The control law may generate large sweeping arcs to the goal pose, depending on the initial robot orientation and k_phi, k_delta. | 
-| `initial_rotation_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `initial_rotation` is enabled. | 
-| `final_rotation` | Similar to `initial_rotation`, the control law can generate large arcs when the goal orientation is not aligned with the path. If this is enabled, the final pose will be ignored and the robot will follow the orientation of he path and will make a final rotation in place to the goal orientation. | 
-| `rotation_scaling_factor` | The scaling factor applied to the rotation in place velocity. | 
+| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. |
+| `k_phi` | Ratio of the rate of change in phi to the rate of change in r. Controls the convergence of the slow subsystem. If this value is equal to zero, the controller will behave as a pure waypoint follower. A high value offers extreme scenario of pose-following where theta is reduced much faster than r. **Note**: This variable is called k1 in earlier versions of the paper. |
+| `k_delta` | Constant factor applied to the heading error feedback. Controls the convergence of the fast subsystem. The bigger the value, the robot converge faster to the reference heading. **Note**: This variable is called k2 in earlier versions of the paper. |
+| `beta` | Constant factor applied to the path curvature. This value must be positive. Determines how fast the velocity drops when the curvature increases. |
+| `lambda` | Constant factor applied to the path curvature. This value must be greater or equal to 1. Determines the sharpness of the curve: higher lambda implies sharper curves. |
+| `v_linear_min` | Minimum linear velocity. Units: meters/sec. |
+| `v_linear_max` | Maximum linear velocity. Units: meters/sec. |
+| `v_angular_max` | Maximum angular velocity produced by the control law. Units: radians/sec. |
+| `slowdown_radius` | Radius around the goal pose in which the robot will start to slow down. Units: meters. |
+| `initial_rotation` | Enable a rotation in place to the goal before starting the path. The control law may generate large sweeping arcs to the goal pose, depending on the initial robot orientation and k_phi, k_delta. |
+| `initial_rotation_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `initial_rotation` is enabled. |
+| `final_rotation` | Similar to `initial_rotation`, the control law can generate large arcs when the goal orientation is not aligned with the path. If this is enabled, the final pose will be ignored and the robot will follow the orientation of he path and will make a final rotation in place to the goal orientation. |
+| `rotation_scaling_factor` | The scaling factor applied to the rotation in place velocity. |
 | `allow_backward` | Whether to allow the robot to move backward. |
 
 ## Topics
 
-| Topic  | Type | Description | 
+| Topic  | Type | Description |
 |-----|----|----|
-| `transformed_global_plan`  | `nav_msgs/Path` | The global plan transformed into the robot frame. | 
-| `local_plan`  | `nav_msgs/Path` | The local plan generated by appliyng iteratively the control law upon a set of motion targets along the global plan. | 
-| `motion_target`  | `geometry_msgs/PointStamped` | The current motion target used by the controller to compute the velocity commands. | 
-| `slowdown`  | `visualization_msgs/Marker` | A flat circle marker of radius slowdown_radius around the motion target. | 
+| `transformed_global_plan`  | `nav_msgs/Path` | The global plan transformed into the robot frame. |
+| `local_plan`  | `nav_msgs/Path` | The local plan generated by appliyng iteratively the control law upon a set of motion targets along the global plan. |
+| `motion_target`  | `geometry_msgs/PointStamped` | The current motion target used by the controller to compute the velocity commands. |
+| `slowdown`  | `visualization_msgs/Marker` | A flat circle marker of radius slowdown_radius around the motion target. |
diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp
index dd4d4df09ce..3d6ad16f33b 100644
--- a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp
+++ b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp
@@ -53,6 +53,7 @@ struct Parameters
   double rotation_scaling_factor;
   bool allow_backward;
   double in_place_collision_resolution;
+  bool use_collision_detection;
 };
 
 /**
diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp
index 3e6857d0287..7a9a9e1c08d 100644
--- a/nav2_graceful_controller/src/graceful_controller.cpp
+++ b/nav2_graceful_controller/src/graceful_controller.cpp
@@ -56,8 +56,10 @@ void GracefulController::configure(
     params_->v_linear_min, params_->v_linear_max, params_->v_angular_max);
 
   // Initialize footprint collision checker
-  collision_checker_ = std::make_unique<nav2_costmap_2d::
-      FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>(costmap_ros_->getCostmap());
+  if (params_->use_collision_detection) {
+    collision_checker_ = std::make_unique<nav2_costmap_2d::
+        FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>(costmap_ros_->getCostmap());
+  }
 
   // Publishers
   transformed_plan_pub_ = node->create_publisher<nav_msgs::msg::Path>("transformed_global_plan", 1);
@@ -177,7 +179,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
       next_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw);
       geometry_msgs::msg::PoseStamped costmap_pose;
       tf2::doTransform(next_pose, costmap_pose, costmap_transform);
-      if (inCollision(
+      if (params_->use_collision_detection && inCollision(
           costmap_pose.pose.position.x, costmap_pose.pose.position.y,
           tf2::getYaw(costmap_pose.pose.orientation)))
       {
@@ -348,7 +350,7 @@ bool GracefulController::simulateTrajectory(
     // Check for collision
     geometry_msgs::msg::PoseStamped global_pose;
     tf2::doTransform(next_pose, global_pose, costmap_transform);
-    if (inCollision(
+    if (params_->use_collision_detection && inCollision(
         global_pose.pose.position.x, global_pose.pose.position.y,
         tf2::getYaw(global_pose.pose.orientation)))
     {
@@ -367,8 +369,9 @@ geometry_msgs::msg::Twist GracefulController::rotateToTarget(double angle_to_tar
   geometry_msgs::msg::Twist vel;
   vel.linear.x = 0.0;
   vel.angular.z = params_->rotation_scaling_factor * angle_to_target * params_->v_angular_max;
-  vel.angular.z = std::copysign(1.0, vel.angular.z) * std::max(abs(vel.angular.z),
-      params_->v_angular_min_in_place);
+  vel.angular.z = std::copysign(1.0, vel.angular.z) * std::max(
+    abs(vel.angular.z),
+    params_->v_angular_min_in_place);
   return vel;
 }
 
diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp
index f8ba2aa075c..7b2f7c42936 100644
--- a/nav2_graceful_controller/src/parameter_handler.cpp
+++ b/nav2_graceful_controller/src/parameter_handler.cpp
@@ -70,6 +70,8 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".allow_backward", rclcpp::ParameterValue(false));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".in_place_collision_resolution", rclcpp::ParameterValue(0.1));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".use_collision_detection", rclcpp::ParameterValue(true));
 
   node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance);
   node->get_parameter(plugin_name_ + ".min_lookahead", params_.min_lookahead);
@@ -103,6 +105,8 @@ ParameterHandler::ParameterHandler(
   node->get_parameter(plugin_name_ + ".allow_backward", params_.allow_backward);
   node->get_parameter(
     plugin_name_ + ".in_place_collision_resolution", params_.in_place_collision_resolution);
+  node->get_parameter(
+    plugin_name_ + ".use_collision_detection", params_.use_collision_detection);
 
   if (params_.initial_rotation && params_.allow_backward) {
     RCLCPP_WARN(
@@ -187,6 +191,8 @@ ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> param
           continue;
         }
         params_.allow_backward = parameter.as_bool();
+      } else if (name == plugin_name_ + ".use_collision_detection") {
+        params_.use_collision_detection = parameter.as_bool();
       }
     }
   }
diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp
index de95598bb0f..98e9dbd4e60 100644
--- a/nav2_graceful_controller/test/test_graceful_controller.cpp
+++ b/nav2_graceful_controller/test/test_graceful_controller.cpp
@@ -248,6 +248,8 @@ TEST(GracefulControllerTest, dynamicParameters) {
     node, "test.initial_rotation", rclcpp::ParameterValue(true));
   nav2_util::declare_parameter_if_not_declared(
     node, "test.allow_backward", rclcpp::ParameterValue(true));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "test.use_collision_detection", rclcpp::ParameterValue(true));
 
   // Create controller
   auto controller = std::make_shared<GMControllerFixture>();
@@ -282,6 +284,7 @@ TEST(GracefulControllerTest, dynamicParameters) {
       rclcpp::Parameter("test.prefer_final_rotation", false),
       rclcpp::Parameter("test.rotation_scaling_factor", 13.0),
       rclcpp::Parameter("test.allow_backward", true),
+      rclcpp::Parameter("test.use_collision_detection", false),
       rclcpp::Parameter("test.in_place_collision_resolution", 15.0)});
 
   // Spin
@@ -305,6 +308,7 @@ TEST(GracefulControllerTest, dynamicParameters) {
   EXPECT_EQ(node->get_parameter("test.prefer_final_rotation").as_bool(), false);
   EXPECT_EQ(node->get_parameter("test.rotation_scaling_factor").as_double(), 13.0);
   EXPECT_EQ(node->get_parameter("test.allow_backward").as_bool(), true);
+  EXPECT_EQ(node->get_parameter("test.use_collision_detection").as_bool(), false);
   EXPECT_EQ(node->get_parameter("test.in_place_collision_resolution").as_double(), 15.0);
 
   // Set initial rotation to true
diff --git a/nav2_lifecycle_manager/README.md b/nav2_lifecycle_manager/README.md
index e4622119885..e001c98d691 100644
--- a/nav2_lifecycle_manager/README.md
+++ b/nav2_lifecycle_manager/README.md
@@ -15,4 +15,4 @@ The diagram below shows an _example_ of a list of managed nodes, and how it inte
 
 The UML diagram below shows the sequence of service calls once the _startup_ is requested from the lifecycle manager.
 
-<img src="./doc/uml_lifecycle_manager.JPG" title="Lifecycle manager UML diagram" width="100%" align="middle">
\ No newline at end of file
+<img src="./doc/uml_lifecycle_manager.JPG" title="Lifecycle manager UML diagram" width="100%" align="middle">
diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
index a42350d5c06..2f3aa38bf2a 100644
--- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
+++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
@@ -25,6 +25,7 @@
 
 #include "nav2_util/lifecycle_service_client.hpp"
 #include "nav2_util/node_thread.hpp"
+#include "nav2_util/service_server.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "std_srvs/srv/empty.hpp"
 #include "nav2_msgs/srv/manage_lifecycle_nodes.hpp"
@@ -74,8 +75,8 @@ class LifecycleManager : public rclcpp::Node
   std::unique_ptr<nav2_util::NodeThread> service_thread_;
 
   // The services provided by this node
-  rclcpp::Service<ManageLifecycleNodes>::SharedPtr manager_srv_;
-  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr is_active_srv_;
+  nav2_util::ServiceServer<ManageLifecycleNodes>::SharedPtr manager_srv_;
+  nav2_util::ServiceServer<std_srvs::srv::Trigger>::SharedPtr is_active_srv_;
   /**
    * @brief Lifecycle node manager callback function
    * @param request_header Header of the service request
@@ -148,6 +149,12 @@ class LifecycleManager : public rclcpp::Node
    */
   void createLifecycleServiceClients();
 
+  // Support function for creating service servers
+  /**
+   * @brief Support function for creating service servers
+   */
+  void createLifecycleServiceServers();
+
   // Support functions for shutdown
   /**
    * @brief Support function for shutdown
diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp
index 80e9305e9e0..ddcfe26daca 100644
--- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp
+++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp
@@ -108,8 +108,8 @@ class LifecycleManagerClient
   // The node to use for the service call
   rclcpp::Node::SharedPtr node_;
 
-  std::shared_ptr<nav2_util::ServiceClient<ManageLifecycleNodes>> manager_client_;
-  std::shared_ptr<nav2_util::ServiceClient<std_srvs::srv::Trigger>> is_active_client_;
+  nav2_util::ServiceClient<ManageLifecycleNodes>::SharedPtr manager_client_;
+  nav2_util::ServiceClient<std_srvs::srv::Trigger>::SharedPtr is_active_client_;
   std::string manage_service_name_;
   std::string active_service_name_;
 };
diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp
index e37f53cf1c1..de1d918ae8e 100644
--- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp
+++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp
@@ -61,17 +61,6 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options)
   get_parameter("attempt_respawn_reconnection", attempt_respawn_reconnection_);
 
   callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
-  manager_srv_ = create_service<ManageLifecycleNodes>(
-    get_name() + std::string("/manage_nodes"),
-    std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3),
-    rclcpp::SystemDefaultsQoS(),
-    callback_group_);
-
-  is_active_srv_ = create_service<std_srvs::srv::Trigger>(
-    get_name() + std::string("/is_active"),
-    std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3),
-    rclcpp::SystemDefaultsQoS(),
-    callback_group_);
 
   transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE;
   transition_state_map_[Transition::TRANSITION_CLEANUP] = State::PRIMARY_STATE_UNCONFIGURED;
@@ -92,6 +81,7 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options)
     [this]() -> void {
       init_timer_->cancel();
       createLifecycleServiceClients();
+      createLifecycleServiceServers();
       if (autostart_) {
         init_timer_ = this->create_wall_timer(
           0s,
@@ -201,6 +191,25 @@ LifecycleManager::createLifecycleServiceClients()
   }
 }
 
+void
+LifecycleManager::createLifecycleServiceServers()
+{
+  message("Creating and initializing lifecycle service servers");
+  manager_srv_ = std::make_shared<nav2_util::ServiceServer<ManageLifecycleNodes>>(
+    get_name() + std::string("/manage_nodes"),
+    shared_from_this(),
+    std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3),
+    rclcpp::SystemDefaultsQoS(),
+    callback_group_);
+
+  is_active_srv_ = std::make_shared<nav2_util::ServiceServer<std_srvs::srv::Trigger>>(
+    get_name() + std::string("/is_active"),
+    shared_from_this(),
+    std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3),
+    rclcpp::SystemDefaultsQoS(),
+    callback_group_);
+}
+
 void
 LifecycleManager::destroyLifecycleServiceClients()
 {
diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp
index 4f2f14ed544..7c287f2defb 100644
--- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp
+++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp
@@ -37,9 +37,9 @@ LifecycleManagerClient::LifecycleManagerClient(
 
   // Create the service clients
   manager_client_ = std::make_shared<nav2_util::ServiceClient<ManageLifecycleNodes>>(
-    manage_service_name_, node_);
+    manage_service_name_, node_, true /*creates and spins an internal executor*/);
   is_active_client_ = std::make_shared<nav2_util::ServiceClient<std_srvs::srv::Trigger>>(
-    active_service_name_, node_);
+    active_service_name_, node_, true /*creates and spins an internal executor*/);
 }
 
 bool
diff --git a/nav2_lifecycle_manager/test/launch_bond_test.py b/nav2_lifecycle_manager/test/launch_bond_test.py
index 728a061001a..4f4687c576a 100755
--- a/nav2_lifecycle_manager/test/launch_bond_test.py
+++ b/nav2_lifecycle_manager/test/launch_bond_test.py
@@ -16,14 +16,13 @@
 import os
 import sys
 
-from launch import LaunchDescription
-from launch import LaunchService
+from launch import LaunchDescription, LaunchService
 from launch.actions import ExecuteProcess
 from launch_ros.actions import Node
 from launch_testing.legacy import LaunchTestService
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     return LaunchDescription(
         [
             Node(
@@ -41,7 +40,7 @@ def generate_launch_description():
     )
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
     testExecutable = os.getenv('TEST_EXECUTABLE')
diff --git a/nav2_lifecycle_manager/test/launch_lifecycle_test.py b/nav2_lifecycle_manager/test/launch_lifecycle_test.py
index 06582361858..66d013f57bc 100755
--- a/nav2_lifecycle_manager/test/launch_lifecycle_test.py
+++ b/nav2_lifecycle_manager/test/launch_lifecycle_test.py
@@ -16,14 +16,13 @@
 import os
 import sys
 
-from launch import LaunchDescription
-from launch import LaunchService
+from launch import LaunchDescription, LaunchService
 from launch.actions import ExecuteProcess
 from launch_ros.actions import Node
 from launch_testing.legacy import LaunchTestService
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     return LaunchDescription(
         [
             Node(
@@ -42,7 +41,7 @@ def generate_launch_description():
     )
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
     testExecutable = os.getenv('TEST_EXECUTABLE')
diff --git a/nav2_loopback_sim/launch/loopback_simulation.launch.py b/nav2_loopback_sim/launch/loopback_simulation.launch.py
index 0124cb848f5..18756f240d0 100644
--- a/nav2_loopback_sim/launch/loopback_simulation.launch.py
+++ b/nav2_loopback_sim/launch/loopback_simulation.launch.py
@@ -15,14 +15,13 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     bringup_dir = get_package_share_directory('nav2_bringup')
     params_file = LaunchConfiguration('params_file')
     declare_params_file_cmd = DeclareLaunchArgument(
diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py
index 6f75ef442cc..53d67540214 100644
--- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py
+++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py
@@ -14,28 +14,26 @@
 
 
 import math
+from typing import Optional
 
-from geometry_msgs.msg import PoseWithCovarianceStamped, Twist, TwistStamped
-from geometry_msgs.msg import Quaternion, TransformStamped, Vector3
+from geometry_msgs.msg import (PoseWithCovarianceStamped, Quaternion, TransformStamped, Twist,
+                               TwistStamped, Vector3)
 from nav2_simple_commander.line_iterator import LineIterator
 from nav_msgs.msg import Odometry
 from nav_msgs.srv import GetMap
+import numpy as np
 import rclpy
 from rclpy.duration import Duration
 from rclpy.node import Node
 from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
+from rclpy.timer import Timer
 from rosgraph_msgs.msg import Clock
 from sensor_msgs.msg import LaserScan
 from tf2_ros import Buffer, TransformBroadcaster, TransformListener
 import tf_transformations
 
-from .utils import (
-    addYawToQuat,
-    getMapOccupancy,
-    matrixToTransform,
-    transformStampedToMatrix,
-    worldToMap,
-)
+from .utils import (addYawToQuat, getMapOccupancy, matrixToTransform, transformStampedToMatrix,
+                    worldToMap)
 
 """
 This is a loopback simulator that replaces a physics simulator to create a
@@ -49,15 +47,15 @@
 
 class LoopbackSimulator(Node):
 
-    def __init__(self):
+    def __init__(self) -> None:
         super().__init__(node_name='loopback_simulator')
         self.curr_cmd_vel = None
         self.curr_cmd_vel_time = self.get_clock().now()
-        self.initial_pose = None
-        self.timer = None
+        self.initial_pose: PoseWithCovarianceStamped = None
+        self.timer: Optional[Timer] = None
         self.setupTimer = None
         self.map = None
-        self.mat_base_to_laser = None
+        self.mat_base_to_laser: Optional[np.ndarray[np.float64, np.dtype[np.float64]]] = None
 
         self.declare_parameter('update_duration', 0.01)
         self.update_dur = self.get_parameter('update_duration').get_parameter_value().double_value
@@ -131,7 +129,7 @@ def __init__(self):
 
         self.info('Loopback simulator initialized')
 
-    def getBaseToLaserTf(self):
+    def getBaseToLaserTf(self) -> None:
         try:
             # Wait for transform and lookup
             transform = self.tf_buffer.lookup_transform(
@@ -139,20 +137,20 @@ def getBaseToLaserTf(self):
             self.mat_base_to_laser = transformStampedToMatrix(transform)
 
         except Exception as ex:
-            self.get_logger().error('Transform lookup failed: %s' % str(ex))
+            self.get_logger().error(f'Transform lookup failed: {str(ex)}')
 
-    def setupTimerCallback(self):
+    def setupTimerCallback(self) -> None:
         # Publish initial identity odom transform & laser scan to warm up system
         self.tf_broadcaster.sendTransform(self.t_odom_to_base_link)
         if self.mat_base_to_laser is None:
             self.getBaseToLaserTf()
 
-    def clockTimerCallback(self):
+    def clockTimerCallback(self) -> None:
         msg = Clock()
         msg.clock = self.get_clock().now().to_msg()
         self.clock_pub.publish(msg)
 
-    def cmdVelCallback(self, msg):
+    def cmdVelCallback(self, msg: Twist) -> None:
         self.debug('Received cmd_vel')
         if self.initial_pose is None:
             # Don't consider velocities before the initial pose is set
@@ -160,7 +158,7 @@ def cmdVelCallback(self, msg):
         self.curr_cmd_vel = msg
         self.curr_cmd_vel_time = self.get_clock().now()
 
-    def cmdVelStampedCallback(self, msg):
+    def cmdVelStampedCallback(self, msg: TwistStamped) -> None:
         self.debug('Received cmd_vel')
         if self.initial_pose is None:
             # Don't consider velocities before the initial pose is set
@@ -168,7 +166,7 @@ def cmdVelStampedCallback(self, msg):
         self.curr_cmd_vel = msg.twist
         self.curr_cmd_vel_time = rclpy.time.Time.from_msg(msg.header.stamp)
 
-    def initialPoseCallback(self, msg):
+    def initialPoseCallback(self, msg: PoseWithCovarianceStamped) -> None:
         self.info('Received initial pose!')
         if self.initial_pose is None:
             # Initialize transforms (map->odom as input pose, odom->base_link start from identity)
@@ -207,7 +205,7 @@ def initialPoseCallback(self, msg):
             tf_transformations.concatenate_matrices(mat_map_to_base_link, mat_base_link_to_odom)
         self.t_map_to_odom.transform = matrixToTransform(mat_map_to_odom)
 
-    def timerCallback(self):
+    def timerCallback(self) -> None:
         # If no data, just republish existing transforms without change
         one_sec = Duration(seconds=1)
         if self.curr_cmd_vel is None or self.get_clock().now() - self.curr_cmd_vel_time > one_sec:
@@ -232,7 +230,7 @@ def timerCallback(self):
         self.publishTransforms(self.t_map_to_odom, self.t_odom_to_base_link)
         self.publishOdometry(self.t_odom_to_base_link)
 
-    def publishLaserScan(self):
+    def publishLaserScan(self) -> None:
         # Publish a bogus laser scan for collision monitor
         self.scan_msg = LaserScan()
         self.scan_msg.header.stamp = (self.get_clock().now()).to_msg()
@@ -252,7 +250,8 @@ def publishLaserScan(self):
         self.getLaserScan(num_samples)
         self.scan_pub.publish(self.scan_msg)
 
-    def publishTransforms(self, map_to_odom, odom_to_base_link):
+    def publishTransforms(self, map_to_odom: TransformStamped,
+                          odom_to_base_link: TransformStamped) -> None:
         map_to_odom.header.stamp = \
             (self.get_clock().now() + Duration(seconds=self.update_dur)).to_msg()
         odom_to_base_link.header.stamp = self.get_clock().now().to_msg()
@@ -260,7 +259,7 @@ def publishTransforms(self, map_to_odom, odom_to_base_link):
             self.tf_broadcaster.sendTransform(map_to_odom)
         self.tf_broadcaster.sendTransform(odom_to_base_link)
 
-    def publishOdometry(self, odom_to_base_link):
+    def publishOdometry(self, odom_to_base_link: TransformStamped) -> None:
         odom = Odometry()
         odom.header.stamp = self.get_clock().now().to_msg()
         odom.header.frame_id = 'odom'
@@ -271,22 +270,23 @@ def publishOdometry(self, odom_to_base_link):
         odom.twist.twist = self.curr_cmd_vel
         self.odom_pub.publish(odom)
 
-    def info(self, msg):
+    def info(self, msg: str) -> None:
         self.get_logger().info(msg)
         return
 
-    def debug(self, msg):
+    def debug(self, msg: str) -> None:
         self.get_logger().debug(msg)
         return
 
-    def getMap(self):
+    def getMap(self) -> None:
         request = GetMap.Request()
         if self.map_client.wait_for_service(timeout_sec=5.0):
             # Request to get map
             future = self.map_client.call_async(request)
             rclpy.spin_until_future_complete(self, future)
-            if future.result() is not None:
-                self.map = future.result().map
+            result = future.result()
+            if result is not None:
+                self.map = result.map
                 self.get_logger().info('Laser scan will be populated using map data')
             else:
                 self.get_logger().warn(
@@ -299,7 +299,7 @@ def getMap(self):
                 'Laser scan will be populated using max range'
             )
 
-    def getLaserPose(self):
+    def getLaserPose(self) -> tuple[float, float, float]:
         mat_map_to_odom = transformStampedToMatrix(self.t_map_to_odom)
         mat_odom_to_base = transformStampedToMatrix(self.t_odom_to_base_link)
 
@@ -321,7 +321,7 @@ def getLaserPose(self):
 
         return x, y, theta
 
-    def getLaserScan(self, num_samples):
+    def getLaserScan(self, num_samples: int) -> None:
         if self.map is None or self.initial_pose is None or self.mat_base_to_laser is None:
             self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples
             return
@@ -363,7 +363,7 @@ def getLaserScan(self, num_samples):
                 line_iterator.advance()
 
 
-def main():
+def main() -> None:
     rclpy.init()
     loopback_simulator = LoopbackSimulator()
     rclpy.spin(loopback_simulator)
diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py
index 468103e94e8..be3b4e73c29 100644
--- a/nav2_loopback_sim/nav2_loopback_sim/utils.py
+++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py
@@ -16,16 +16,16 @@
 import math
 
 from geometry_msgs.msg import Quaternion, Transform
+from nav_msgs.msg import OccupancyGrid
 import numpy as np
 import tf_transformations
 
-
 """
 Transformation utilities for the loopback simulator
 """
 
 
-def addYawToQuat(quaternion, yaw_to_add):
+def addYawToQuat(quaternion: Quaternion, yaw_to_add: float) -> Quaternion:
     q = [quaternion.x, quaternion.y, quaternion.z, quaternion.w]
     q2 = tf_transformations.quaternion_from_euler(0.0, 0.0, yaw_to_add)
     q_new = tf_transformations.quaternion_multiply(q, q2)
@@ -37,7 +37,7 @@ def addYawToQuat(quaternion, yaw_to_add):
     return new_quaternion
 
 
-def transformStampedToMatrix(transform):
+def transformStampedToMatrix(transform: Transform) -> np.ndarray[np.float64, np.dtype[np.float64]]:
     translation = transform.transform.translation
     rotation = transform.transform.rotation
     matrix = np.eye(4)
@@ -54,7 +54,7 @@ def transformStampedToMatrix(transform):
     return matrix
 
 
-def matrixToTransform(matrix):
+def matrixToTransform(matrix: np.ndarray[np.float64, np.dtype[np.float64]]) -> Transform:
     transform = Transform()
     transform.translation.x = matrix[0, 3]
     transform.translation.y = matrix[1, 3]
@@ -67,11 +67,11 @@ def matrixToTransform(matrix):
     return transform
 
 
-def worldToMap(world_x, world_y, map_msg):
+def worldToMap(world_x: float, world_y: float, map_msg: OccupancyGrid) -> tuple[int, int]:
     map_x = int(math.floor((world_x - map_msg.info.origin.position.x) / map_msg.info.resolution))
     map_y = int(math.floor((world_y - map_msg.info.origin.position.y) / map_msg.info.resolution))
     return map_x, map_y
 
 
-def getMapOccupancy(x, y, map_msg):
+def getMapOccupancy(x: int, y: int, map_msg: OccupancyGrid):  # type: ignore[no-untyped-def]
     return map_msg.data[y * map_msg.info.width + x]
diff --git a/nav2_loopback_sim/setup.py b/nav2_loopback_sim/setup.py
index 0d56733614b..8d25201e070 100644
--- a/nav2_loopback_sim/setup.py
+++ b/nav2_loopback_sim/setup.py
@@ -3,7 +3,6 @@
 
 from setuptools import setup
 
-
 package_name = 'nav2_loopback_sim'
 
 setup(
diff --git a/nav2_loopback_sim/test/test_copyright.py b/nav2_loopback_sim/test/test_copyright.py
index cc8ff03f79a..1f869834559 100644
--- a/nav2_loopback_sim/test/test_copyright.py
+++ b/nav2_loopback_sim/test/test_copyright.py
@@ -18,6 +18,6 @@
 
 @pytest.mark.copyright
 @pytest.mark.linter
-def test_copyright():
+def test_copyright() -> None:
     rc = main(argv=['.', 'test'])
     assert rc == 0, 'Found errors'
diff --git a/nav2_loopback_sim/test/test_flake8.py b/nav2_loopback_sim/test/test_flake8.py
index 26030113cce..fe9aa6a300c 100644
--- a/nav2_loopback_sim/test/test_flake8.py
+++ b/nav2_loopback_sim/test/test_flake8.py
@@ -18,8 +18,7 @@
 
 @pytest.mark.flake8
 @pytest.mark.linter
-def test_flake8():
+def test_flake8() -> None:
     rc, errors = main_with_errors(argv=[])
-    assert rc == 0, 'Found %d code style errors / warnings:\n' % len(
-        errors
-    ) + '\n'.join(errors)
+    assert rc == 0, f'Found {len(errors)} code style errors / warnings:\n' + \
+        '\n'.join(errors)
diff --git a/nav2_loopback_sim/test/test_pep257.py b/nav2_loopback_sim/test/test_pep257.py
index b234a3840f4..b6808e1d80f 100644
--- a/nav2_loopback_sim/test/test_pep257.py
+++ b/nav2_loopback_sim/test/test_pep257.py
@@ -18,6 +18,6 @@
 
 @pytest.mark.linter
 @pytest.mark.pep257
-def test_pep257():
+def test_pep257() -> None:
     rc = main(argv=['.', 'test'])
     assert rc == 0, 'Found code style errors / warnings'
diff --git a/nav2_map_server/README.md b/nav2_map_server/README.md
index 4e811a24277..c095898b639 100644
--- a/nav2_map_server/README.md
+++ b/nav2_map_server/README.md
@@ -96,7 +96,7 @@ $ process_with_multiple_map_servers __params:=combined_params.yaml
 
 
 The parameter for the initial map (yaml_filename) has to be set, but an empty string can be used if no initial map should be loaded. In this case, no map is loaded during
-on_configure or published during on_activate. The _load_map_-service should the be used to load and publish a map. 
+on_configure or published during on_activate. The _load_map_-service should the be used to load and publish a map.
 
 
 #### Map Saver
@@ -140,4 +140,3 @@ Service usage examples:
 $ ros2 service call /map_server/load_map nav2_msgs/srv/LoadMap "{map_url: /ros/maps/map.yaml}"
 $ ros2 service call /map_saver/save_map nav2_msgs/srv/SaveMap "{map_topic: map, map_url: my_map, image_format: pgm, map_mode: trinary, free_thresh: 0.25, occupied_thresh: 0.65}"
 ```
-
diff --git a/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake b/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake
index c04356b4aa8..e86f249afde 100644
--- a/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake
+++ b/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake
@@ -31,4 +31,4 @@ find_library(GRAPHICSMAGICKCPP_LIBRARIES
 find_package_handle_standard_args(
   GRAPHICSMAGICKCPP
   GRAPHICSMAGICKCPP_LIBRARIES
-  GRAPHICSMAGICKCPP_INCLUDE_DIRS)
\ No newline at end of file
+  GRAPHICSMAGICKCPP_INCLUDE_DIRS)
diff --git a/nav2_map_server/include/nav2_map_server/map_saver.hpp b/nav2_map_server/include/nav2_map_server/map_saver.hpp
index 18c26be3a71..123e6723b80 100644
--- a/nav2_map_server/include/nav2_map_server/map_saver.hpp
+++ b/nav2_map_server/include/nav2_map_server/map_saver.hpp
@@ -23,6 +23,7 @@
 #include "rclcpp_lifecycle/state.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_msgs/srv/save_map.hpp"
+#include "nav2_util/service_server.hpp"
 
 #include "map_io.hpp"
 
@@ -111,7 +112,8 @@ class MapSaver : public nav2_util::LifecycleNode
   // The name of the service for saving a map from topic
   const std::string save_map_service_name_{"save_map"};
   // A service to save the map to a file at run time (SaveMap)
-  rclcpp::Service<nav2_msgs::srv::SaveMap>::SharedPtr save_map_service_;
+  nav2_util::ServiceServer<nav2_msgs::srv::SaveMap,
+    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr save_map_service_;
 };
 
 }  // namespace nav2_map_server
diff --git a/nav2_map_server/include/nav2_map_server/map_server.hpp b/nav2_map_server/include/nav2_map_server/map_server.hpp
index 23c9fae9d4c..4e6ca327b57 100644
--- a/nav2_map_server/include/nav2_map_server/map_server.hpp
+++ b/nav2_map_server/include/nav2_map_server/map_server.hpp
@@ -19,6 +19,7 @@
 #include <string>
 
 #include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/service_server.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "nav_msgs/srv/get_map.hpp"
 #include "nav2_msgs/srv/load_map.hpp"
@@ -125,10 +126,12 @@ class MapServer : public nav2_util::LifecycleNode
   const std::string load_map_service_name_{"load_map"};
 
   // A service to provide the occupancy grid (GetMap) and the message to return
-  rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr occ_service_;
+  nav2_util::ServiceServer<nav_msgs::srv::GetMap,
+    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr occ_service_;
 
   // A service to load the occupancy grid from file at run time (LoadMap)
-  rclcpp::Service<nav2_msgs::srv::LoadMap>::SharedPtr load_map_service_;
+  nav2_util::ServiceServer<nav2_msgs::srv::LoadMap,
+    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr load_map_service_;
 
   // A topic on which the occupancy grid will be published
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr occ_pub_;
diff --git a/nav2_map_server/launch/map_saver_server.launch.py b/nav2_map_server/launch/map_saver_server.launch.py
index aa3f2c4f7bc..c06fe072eb7 100644
--- a/nav2_map_server/launch/map_saver_server.launch.py
+++ b/nav2_map_server/launch/map_saver_server.launch.py
@@ -18,7 +18,7 @@
 import launch_ros.actions
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     # Parameters
     lifecycle_nodes = ['map_saver']
     use_sim_time = True
diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp
index a75d2f4b415..f85b9161e34 100644
--- a/nav2_map_server/src/map_io.cpp
+++ b/nav2_map_server/src/map_io.cpp
@@ -34,6 +34,9 @@
 #ifndef _WIN32
 #include <libgen.h>
 #endif
+
+#include <Eigen/Dense>
+
 #include <iostream>
 #include <string>
 #include <vector>
@@ -225,72 +228,107 @@ void loadMapFromFile(
   // Allocate space to hold the data
   msg.data.resize(msg.info.width * msg.info.height);
 
-  // Copy pixel data into the map structure
-  for (size_t y = 0; y < msg.info.height; y++) {
-    for (size_t x = 0; x < msg.info.width; x++) {
-      auto pixel = img.pixelColor(x, y);
-
-      std::vector<Magick::Quantum> channels = {pixel.redQuantum(), pixel.greenQuantum(),
-        pixel.blueQuantum()};
-      if (load_parameters.mode == MapMode::Trinary && img.matte()) {
-        // To preserve existing behavior, average in alpha with color channels in Trinary mode.
-        // CAREFUL. alpha is inverted from what you might expect. High = transparent, low = opaque
-        channels.push_back(MaxRGB - pixel.alphaQuantum());
-      }
-      double sum = 0;
-      for (auto c : channels) {
-        sum += c;
-      }
-      /// on a scale from 0.0 to 1.0 how bright is the pixel?
-      double shade = Magick::ColorGray::scaleQuantumToDouble(sum / channels.size());
-
-      // If negate is true, we consider blacker pixels free, and whiter
-      // pixels occupied. Otherwise, it's vice versa.
-      /// on a scale from 0.0 to 1.0, how occupied is the map cell (before thresholding)?
-      double occ = (load_parameters.negate ? shade : 1.0 - shade);
-
-      int8_t map_cell;
-      switch (load_parameters.mode) {
-        case MapMode::Trinary:
-          if (load_parameters.occupied_thresh < occ) {
-            map_cell = nav2_util::OCC_GRID_OCCUPIED;
-          } else if (occ < load_parameters.free_thresh) {
-            map_cell = nav2_util::OCC_GRID_FREE;
-          } else {
-            map_cell = nav2_util::OCC_GRID_UNKNOWN;
-          }
-          break;
-        case MapMode::Scale:
-          if (pixel.alphaQuantum() != OpaqueOpacity) {
-            map_cell = nav2_util::OCC_GRID_UNKNOWN;
-          } else if (load_parameters.occupied_thresh < occ) {
-            map_cell = nav2_util::OCC_GRID_OCCUPIED;
-          } else if (occ < load_parameters.free_thresh) {
-            map_cell = nav2_util::OCC_GRID_FREE;
-          } else {
-            map_cell = std::rint(
-              (occ - load_parameters.free_thresh) /
-              (load_parameters.occupied_thresh - load_parameters.free_thresh) * 100.0);
-          }
-          break;
-        case MapMode::Raw: {
-            double occ_percent = std::round(shade * 255);
-            if (nav2_util::OCC_GRID_FREE <= occ_percent &&
-              occ_percent <= nav2_util::OCC_GRID_OCCUPIED)
-            {
-              map_cell = static_cast<int8_t>(occ_percent);
-            } else {
-              map_cell = nav2_util::OCC_GRID_UNKNOWN;
-            }
-            break;
-          }
-        default:
-          throw std::runtime_error("Invalid map mode");
+  // Convert the image to grayscale
+  Magick::Image gray = img;
+  gray.type(Magick::GrayscaleType);
+
+  // Prepare grayscale matrix from image
+  size_t width = gray.columns();
+  size_t height = gray.rows();
+
+  std::vector<uint8_t> buffer(width * height);
+  gray.write(0, 0, width, height, "I", Magick::CharPixel, buffer.data());
+
+  // Map the grayscale buffer to an Eigen matrix (row-major layout)
+  Eigen::Map<Eigen::Matrix<uint8_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+  gray_matrix(buffer.data(), height, width);
+
+  bool has_alpha = img.matte();
+
+  // Handle different map modes with if else condition
+  // Trinary and Scale modes are handled together
+  // because they share a lot of code
+  // Raw mode is handled separately in else if block
+  Eigen::Matrix<int8_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> result(height, width);
+
+  if (load_parameters.mode == MapMode::Trinary || load_parameters.mode == MapMode::Scale) {
+    // Convert grayscale to float in range [0.0, 1.0]
+    Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic,
+      Eigen::RowMajor> normalized = gray_matrix.cast<float>() / 255.0f;
+
+    // Negate the image if specified (e.g. for black=occupied vs. white=occupied convention)
+    if (!load_parameters.negate) {
+      normalized = (1.0f - normalized.array()).matrix();
+    }
+
+    // Compute binary occupancy masks
+    Eigen::Matrix<uint8_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> occupied =
+      (normalized.array() >= load_parameters.occupied_thresh).cast<uint8_t>();
+
+    Eigen::Matrix<uint8_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> free =
+      (normalized.array() <= load_parameters.free_thresh).cast<uint8_t>();
+
+    // Initialize occupancy grid with UNKNOWN values (-1)
+    result.setConstant(nav2_util::OCC_GRID_UNKNOWN);
+
+    // Apply occupied and free cell updates
+    result = (occupied.array() > 0).select(nav2_util::OCC_GRID_OCCUPIED, result);
+    result = (free.array() > 0).select(nav2_util::OCC_GRID_FREE, result);
+
+    // Handle intermediate (gray) values if in Scale mode
+    if (load_parameters.mode == MapMode::Scale) {
+      // Create in-between mask
+      auto in_between_mask = (normalized.array() > load_parameters.free_thresh) &&
+        (normalized.array() < load_parameters.occupied_thresh);
+
+      if (in_between_mask.any()) {
+        // Scale in-between values to [0,100] range
+        Eigen::ArrayXXf scaled_float = ((normalized.array() - load_parameters.free_thresh) /
+          (load_parameters.occupied_thresh - load_parameters.free_thresh)) * 100.0f;
+
+        // Round and cast to int8_t
+        Eigen::Matrix<int8_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> scaled_int =
+          scaled_float.array().round().cast<int8_t>();
+
+        result = in_between_mask.select(scaled_int, result);
       }
-      msg.data[msg.info.width * (msg.info.height - y - 1) + x] = map_cell;
     }
+
+    // Apply alpha transparency mask: mark transparent cells as UNKNOWN
+    if (has_alpha) {
+      // Allocate buffer only once and map directly to Eigen without extra copy
+      std::vector<uint8_t> alpha_buf(width * height);
+      img.write(0, 0, width, height, "A", Magick::CharPixel, alpha_buf.data());
+
+      // Map alpha buffer as Eigen::Array for efficient elementwise ops
+      Eigen::Map<Eigen::Array<uint8_t, Eigen::Dynamic, Eigen::Dynamic,
+        Eigen::RowMajor>> alpha_array(
+        alpha_buf.data(), height, width);
+
+      // Apply mask directly with Eigen::select
+      result = (alpha_array < 255).select(nav2_util::OCC_GRID_UNKNOWN, result);
+    }
+
+  } else if (load_parameters.mode == MapMode::Raw) {
+    // Raw mode: interpret raw image pixel values directly as occupancy values
+    result = gray_matrix.cast<int8_t>();
+
+    // Clamp out-of-bound values (outside [-1, 100]) to UNKNOWN (-1)
+    auto out_of_bounds = (result.array() < nav2_util::OCC_GRID_FREE) ||
+      (result.array() > nav2_util::OCC_GRID_OCCUPIED);
+
+    result = out_of_bounds.select(nav2_util::OCC_GRID_UNKNOWN, result);
+
+  } else {
+    // If the map mode is not recognized, throw an error
+    throw std::runtime_error("Invalid map mode");
   }
 
+  // Flip image vertically (as ROS expects origin at bottom-left)
+  Eigen::Matrix<int8_t, Eigen::Dynamic, Eigen::Dynamic,
+    Eigen::RowMajor> flipped = result.colwise().reverse();
+  std::memcpy(msg.data.data(), flipped.data(), width * height);
+
   // Since loadMapFromFile() does not belong to any node, publishing in a system time.
   rclcpp::Clock clock(RCL_SYSTEM_TIME);
   msg.info.map_load_time = clock.now();
diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp
index 1517e787526..43226a00349 100644
--- a/nav2_map_server/src/map_saver/map_saver.cpp
+++ b/nav2_map_server/src/map_saver/map_saver.cpp
@@ -72,8 +72,10 @@ MapSaver::on_configure(const rclcpp_lifecycle::State & /*state*/)
   map_subscribe_transient_local_ = get_parameter("map_subscribe_transient_local").as_bool();
 
   // Create a service that saves the occupancy grid from map topic to a file
-  save_map_service_ = create_service<nav2_msgs::srv::SaveMap>(
+  save_map_service_ = std::make_shared<nav2_util::ServiceServer<nav2_msgs::srv::SaveMap,
+      std::shared_ptr<nav2_util::LifecycleNode>>>(
     service_prefix + save_map_service_name_,
+    shared_from_this(),
     std::bind(&MapSaver::saveMapCallback, this, _1, _2, _3));
 
   return nav2_util::CallbackReturn::SUCCESS;
diff --git a/nav2_map_server/src/map_server/map_server.cpp b/nav2_map_server/src/map_server/map_server.cpp
index 6f2a6e04837..a9470ef9ce6 100644
--- a/nav2_map_server/src/map_server/map_server.cpp
+++ b/nav2_map_server/src/map_server/map_server.cpp
@@ -108,8 +108,10 @@ MapServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
   const std::string service_prefix = get_name() + std::string("/");
 
   // Create a service that provides the occupancy grid
-  occ_service_ = create_service<nav_msgs::srv::GetMap>(
+  occ_service_ = std::make_shared<nav2_util::ServiceServer<nav_msgs::srv::GetMap,
+      std::shared_ptr<nav2_util::LifecycleNode>>>(
     service_prefix + std::string(service_name_),
+    shared_from_this(),
     std::bind(&MapServer::getMapCallback, this, _1, _2, _3));
 
   // Create a publisher using the QoS settings to emulate a ROS1 latched topic
@@ -118,8 +120,10 @@ MapServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
     rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
 
   // Create a service that loads the occupancy grid from a file
-  load_map_service_ = create_service<nav2_msgs::srv::LoadMap>(
+  load_map_service_ = std::make_shared<nav2_util::ServiceServer<nav2_msgs::srv::LoadMap,
+      std::shared_ptr<nav2_util::LifecycleNode>>>(
     service_prefix + std::string(load_map_service_name_),
+    shared_from_this(),
     std::bind(&MapServer::loadMapCallback, this, _1, _2, _3));
 
   return nav2_util::CallbackReturn::SUCCESS;
diff --git a/nav2_map_server/test/component/test_map_saver_launch.py b/nav2_map_server/test/component/test_map_saver_launch.py
index e33e03a9864..35fba24fe4d 100755
--- a/nav2_map_server/test/component/test_map_saver_launch.py
+++ b/nav2_map_server/test/component/test_map_saver_launch.py
@@ -17,17 +17,17 @@
 import os
 import sys
 
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import ExecuteProcess
-from launch.actions import IncludeLaunchDescription
+from launch import LaunchDescription, LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_testing.legacy import LaunchTestService
 
 
-def main(argv=sys.argv[1:]):
-    launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'map_saver_node.launch.py')
-    testExecutable = os.getenv('TEST_EXECUTABLE')
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
+    launchDir = os.getenv('TEST_LAUNCH_DIR', '')
+    testExecutable = os.getenv('TEST_EXECUTABLE', '')
+
+    launchFile = os.path.join(launchDir, 'map_saver_node.launch.py')
     ld = LaunchDescription(
         [
             IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])),
@@ -41,7 +41,7 @@ def main(argv=sys.argv[1:]):
     lts.add_test_action(ld, test1_action)
     ls = LaunchService(argv=argv)
     ls.include_launch_description(ld)
-    os.chdir(os.getenv('TEST_LAUNCH_DIR'))
+    os.chdir(launchDir)
     return lts.run(ls)
 
 
diff --git a/nav2_map_server/test/component/test_map_saver_node.cpp b/nav2_map_server/test/component/test_map_saver_node.cpp
index 40f85c7701f..e9cb1852b25 100644
--- a/nav2_map_server/test/component/test_map_saver_node.cpp
+++ b/nav2_map_server/test/component/test_map_saver_node.cpp
@@ -212,7 +212,7 @@ TEST_F(MapSaverTestFixture, SaveMapInvalidParameters)
   ASSERT_EQ(resp->result, false);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_map_server/test/component/test_map_server_launch.py b/nav2_map_server/test/component/test_map_server_launch.py
index 473feea97eb..681c567f2e1 100755
--- a/nav2_map_server/test/component/test_map_server_launch.py
+++ b/nav2_map_server/test/component/test_map_server_launch.py
@@ -17,17 +17,17 @@
 import os
 import sys
 
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import ExecuteProcess
-from launch.actions import IncludeLaunchDescription
+from launch import LaunchDescription, LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_testing.legacy import LaunchTestService
 
 
-def main(argv=sys.argv[1:]):
-    launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'map_server_node.launch.py')
-    testExecutable = os.getenv('TEST_EXECUTABLE')
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
+    launchDir = os.getenv('TEST_LAUNCH_DIR', '')
+    testExecutable = os.getenv('TEST_EXECUTABLE', '')
+
+    launchFile = os.path.join(launchDir, 'map_server_node.launch.py')
     ld = LaunchDescription(
         [
             IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])),
@@ -41,7 +41,7 @@ def main(argv=sys.argv[1:]):
     lts.add_test_action(ld, test1_action)
     ls = LaunchService(argv=argv)
     ls.include_launch_description(ld)
-    os.chdir(os.getenv('TEST_LAUNCH_DIR'))
+    os.chdir(launchDir)
     return lts.run(ls)
 
 
diff --git a/nav2_map_server/test/component/test_map_server_node.cpp b/nav2_map_server/test/component/test_map_server_node.cpp
index ccebe4fdded..bb5dbb06683 100644
--- a/nav2_map_server/test/component/test_map_server_node.cpp
+++ b/nav2_map_server/test/component/test_map_server_node.cpp
@@ -227,7 +227,7 @@ TEST_F(MapServerTestFixture, NoInitialMap)
   verifyMapMsg(load_res->map);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_map_server/test/test_launch_files/map_saver_node.launch.py b/nav2_map_server/test/test_launch_files/map_saver_node.launch.py
index a0ac0aeb8a0..fba6faa1953 100644
--- a/nav2_map_server/test/test_launch_files/map_saver_node.launch.py
+++ b/nav2_map_server/test/test_launch_files/map_saver_node.launch.py
@@ -21,9 +21,9 @@
 import launch_ros.actions
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     map_publisher = (
-        f"{os.path.dirname(os.getenv('TEST_EXECUTABLE'))}/test_map_saver_publisher"
+        f"{os.path.dirname(os.getenv('TEST_EXECUTABLE', ''))}/test_map_saver_publisher"
     )
 
     ld = LaunchDescription()
@@ -32,7 +32,7 @@ def generate_launch_description():
         package='nav2_map_server',
         executable='map_saver_server',
         output='screen',
-        parameters=[os.path.join(os.getenv('TEST_DIR'), 'map_saver_params.yaml')],
+        parameters=[os.path.join(os.getenv('TEST_DIR', ''), 'map_saver_params.yaml')],
     )
 
     map_publisher_cmd = ExecuteProcess(cmd=[map_publisher])
diff --git a/nav2_map_server/test/test_launch_files/map_server_node.launch.py b/nav2_map_server/test/test_launch_files/map_server_node.launch.py
index 813abf7ea14..0a54a217827 100644
--- a/nav2_map_server/test/test_launch_files/map_server_node.launch.py
+++ b/nav2_map_server/test/test_launch_files/map_server_node.launch.py
@@ -20,7 +20,7 @@
 import launch_ros.actions
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     return LaunchDescription(
         [
             launch_ros.actions.Node(
@@ -28,7 +28,7 @@ def generate_launch_description():
                 executable='map_server',
                 output='screen',
                 parameters=[
-                    os.path.join(os.getenv('TEST_DIR'), 'map_server_params.yaml')
+                    os.path.join(os.getenv('TEST_DIR', ''), 'map_server_params.yaml')
                 ],
             )
         ]
diff --git a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp
index 024e1d41497..c64bda2e5a4 100644
--- a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp
+++ b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp
@@ -168,7 +168,7 @@ TEST_F(InfoServerTester, testCostmapFilterInfoDeactivateActivate)
   EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_map_server/test/unit/test_map_io.cpp b/nav2_map_server/test/unit/test_map_io.cpp
index 7b57bb3d883..d0142e711c8 100644
--- a/nav2_map_server/test/unit/test_map_io.cpp
+++ b/nav2_map_server/test/unit/test_map_io.cpp
@@ -330,7 +330,7 @@ TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldExpandHomeSequenceWhenHome
   ASSERT_EQ(expectedOutputFileName, expand_user_home_dir_if_needed(fileName, "/home/user"));
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md
index e8ca1e5afa9..3b11644e19f 100644
--- a/nav2_mppi_controller/README.md
+++ b/nav2_mppi_controller/README.md
@@ -58,6 +58,8 @@ This process is then repeated a number of times and returns a converged solution
  | visualize                  | bool   | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging.                                                                                                                                       |
  | retry_attempt_limit        | int    | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure.                                                                                                                                                                                                       |
  | regenerate_noises          | bool   | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. |
+ | publish_optimal_trajectory | bool   | Publishes the full optimal trajectory sequence each control iteration for downstream  control systems, collision checkers, etc to have context beyond the next timestep. |
+
 
 #### Trajectory Visualizer
  | Parameter             | Type   | Definition                                                                                                  |
@@ -84,7 +86,7 @@ This process is then repeated a number of times and returns a converged solution
  | Parameter             | Type   | Definition                                                                                                  |
  | ---------------       | ------ | ----------------------------------------------------------------------------------------------------------- |
  | cost_weight           | double | Default 4.0. Weight to apply to critic term.                                                                |
- | cost_power            | int    | Default 1. Power order to apply to term.   
+ | cost_power            | int    | Default 1. Power order to apply to term.
 
 #### Goal Angle Critic
  | Parameter                        | Type   | Definition                                                                                                  |
@@ -113,7 +115,7 @@ Uses estimated distances from obstacles using cost and inflation parameters to a
  | cost_power           | int    | Default 1. Power order to apply to term.                                                                    |
  | collision_cost       | double | Default 100000.0. Cost to apply to a true collision in a trajectory.                                          |
  | collision_margin_distance   | double    | Default 0.10. Margin distance from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable. |
- | near_goal_distance          | double    | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles.   
+ | near_goal_distance          | double    | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles.
  | inflation_layer_name        | string    | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. |
 
 #### Cost Critic
@@ -123,11 +125,11 @@ Uses inflated costmap cost directly to avoid obstacles
  | Parameter            | Type   | Definition                                                                                                  |
  | ---------------      | ------ | ----------------------------------------------------------------------------------------------------------- |
  | consider_footprint   | bool   | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. |
- | cost_weight          | double | Default 3.81. Weight to apply to critic to avoid obstacles.                                       | 
+ | cost_weight          | double | Default 3.81. Weight to apply to critic to avoid obstacles.                                       |
  | cost_power           | int    | Default 1. Power order to apply to term.                                                                    |
  | collision_cost       | double | Default 1000000.0. Cost to apply to a true collision in a trajectory.                                          |
  | critical_cost       | double | Default 300.0. Cost to apply to a pose with any point in in inflated space to prefer distance from obstacles.                                          |
- | near_goal_distance          | double    | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles.   
+ | near_goal_distance          | double    | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles.
  | inflation_layer_name        | string    | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. |
  | trajectory_point_step      | int | Default 2. Step of trajectory points to evaluate for costs since otherwise so dense represents multiple points for a single costmap cell.   |
 
@@ -159,7 +161,7 @@ Uses inflated costmap cost directly to avoid obstacles
  | cost_weight           | double | Default 5.0. Weight to apply to critic term.                                                                |
  | cost_power            | int    | Default 1. Power order to apply to term.   |
  | offset_from_furthest  | int    | Default 6. Number of path points after furthest one any trajectory achieves to drive path tracking relative to.     |
- | threshold_to_consider        | float  | Default 1.4. Distance between robot and goal above which path follow cost stops being considered  | 
+ | threshold_to_consider        | float  | Default 1.4. Distance between robot and goal above which path follow cost stops being considered  |
 
 #### Prefer Forward Critic
  | Parameter             | Type   | Definition                                                                                                  |
@@ -302,7 +304,7 @@ Visualization of the trajectories using `visualize` uses compute resources to ba
 
 The most common parameters you might want to start off changing are the velocity profiles (`vx_max`, `vx_min`, `wz_max`, and `vy_max` if holonomic) and the `motion_model` to correspond to your vehicle. Its wise to consider the `prune_distance` of the path plan in proportion to your maximum velocity and prediction horizon. The only deeper parameter that will likely need to be adjusted for your particular settings is the Obstacle critics' `repulsion_weight` since the tuning of this is proportional to your inflation layer's radius. Higher radii should correspond to reduced `repulsion_weight` due to the penalty formation (e.g. `inflation_radius - min_dist_to_obstacle`). If this penalty is too high, the robot will slow significantly when entering cost-space from non-cost space or jitter in narrow corridors. It is noteworthy, but likely not necessary to be changed, that the Obstacle critic may use the full footprint information if `consider_footprint = true`, though comes at an increased compute cost.
 
-If you don't require path following behavior (e.g. just want to follow a goal pose and let the model predictive elements decide the best way to accomplish that), you may easily remove the PathAlign, PathFollow and PathAngle critics. 
+If you don't require path following behavior (e.g. just want to follow a goal pose and let the model predictive elements decide the best way to accomplish that), you may easily remove the PathAlign, PathFollow and PathAngle critics.
 
 By default, the controller is tuned and has the capabilities established in the PathAlign/Obstacle critics to generally follow the path closely when no obstacles prevent it, but able to deviate from the path when blocked. See `PathAlignCritic::score()` for details, but it is disabled when the local path is blocked so the obstacle critic takes over in that state.
 
@@ -318,14 +320,14 @@ The Path Follow critic cannot drive velocities greater than the projectable dist
 
 ### Obstacle, Inflation Layer, and Path Following
 
-There also exists a relationship between the costmap configurations and the Obstacle critic configurations. If the Obstacle critic is not well tuned with the costmap parameters (inflation radius, scale) it can cause the robot to wobble significantly as it attempts to take finitely lower-cost trajectories with a slightly lower cost in exchange for jerky motion. The default behavior was tuned for small AMRs (e.g. turtlebots or similar), so if using a larger robot, you may want to reduce the `repulsion_weight` in kind. It may also perform awkward maneuvers when in free-space to try to maximize time in a small pocket of 0-cost over a more natural motion which involves moving into some low-costed region. Finally, it may generally refuse to go into costed space at all when starting in a free 0-cost space if the gain is set disproportionately higher than the Path Follow scoring to encourage the robot to move along the path. This is due to the critic cost of staying in free space becoming more attractive than entering even lightly costed space in exchange for progression along the task. 
+There also exists a relationship between the costmap configurations and the Obstacle critic configurations. If the Obstacle critic is not well tuned with the costmap parameters (inflation radius, scale) it can cause the robot to wobble significantly as it attempts to take finitely lower-cost trajectories with a slightly lower cost in exchange for jerky motion. The default behavior was tuned for small AMRs (e.g. turtlebots or similar), so if using a larger robot, you may want to reduce the `repulsion_weight` in kind. It may also perform awkward maneuvers when in free-space to try to maximize time in a small pocket of 0-cost over a more natural motion which involves moving into some low-costed region. Finally, it may generally refuse to go into costed space at all when starting in a free 0-cost space if the gain is set disproportionately higher than the Path Follow scoring to encourage the robot to move along the path. This is due to the critic cost of staying in free space becoming more attractive than entering even lightly costed space in exchange for progression along the task.
 
 Thus, care should be taken to select weights of the obstacle critic in conjunction with the costmap inflation radius and scale so that a robot does not have such issues. How I (Steve, your friendly neighborhood navigator) tuned this was to first create the appropriate obstacle critic behavior desirable in conjunction with the inflation layer parameters. Its worth noting that the Obstacle critic converts the cost into a distance from obstacles, so the nature of the distribution of costs in the inflation isn't overly significant. However, the inflation radius and the scale will define the cost at the end of the distribution where free-space meets the lowest cost value within the radius. So testing for quality behavior when going over that threshold should be considered.
 
 As you increase or decrease your weights on the Obstacle, you may notice the aforementioned behaviors (e.g. won't overcome free to non-free threshold). To overcome them, increase the FollowPath critic cost to increase the desire for the trajectory planner to continue moving towards the goal. Make sure to not overshoot this though, keep them balanced. A desirable outcome is smooth motion roughly in the center of spaces without significant close interactions with obstacles. It shouldn't be perfectly following a path yet nor should the output velocity be wobbling jaggedly.
 
-Once you have your obstacle avoidance behavior tuned and matched with an appropriate path following penalty, tune the Path Align critic to align with the path. If you design exact-path-alignment behavior, its possible to skip the obstacle critic step as highly tuning the system to follow the path will give it less ability to deviate to avoid obstacles (though it'll slow and stop). Tuning the critic weight for the Obstacle critic high will do the job to avoid near-collisions but the repulsion weight is largely unnecessary to you. For others wanting more dynamic behavior, it _can_ be beneficial to slowly lower the weight on the obstacle critic to give the path alignment critic some more room to work. If your path was generated with a cost-aware planner (like all provided by Nav2) and providing paths sufficiently far from obstacles for your satisfaction, the impact of a slightly reduced Obstacle critic with a Path Alignment critic will do you well. Not over-weighting the path align critic will allow the robot to  deviate from the path to get around dynamic obstacles in the scene or other obstacles not previous considered during path planning. It is subjective as to the best behavior for your application, but it has been shown that MPPI can be an exact path tracker and/or avoid dynamic obstacles very fluidly and everywhere in between. The defaults provided are in the generally right regime for a balanced initial trade-off. 
+Once you have your obstacle avoidance behavior tuned and matched with an appropriate path following penalty, tune the Path Align critic to align with the path. If you design exact-path-alignment behavior, its possible to skip the obstacle critic step as highly tuning the system to follow the path will give it less ability to deviate to avoid obstacles (though it'll slow and stop). Tuning the critic weight for the Obstacle critic high will do the job to avoid near-collisions but the repulsion weight is largely unnecessary to you. For others wanting more dynamic behavior, it _can_ be beneficial to slowly lower the weight on the obstacle critic to give the path alignment critic some more room to work. If your path was generated with a cost-aware planner (like all provided by Nav2) and providing paths sufficiently far from obstacles for your satisfaction, the impact of a slightly reduced Obstacle critic with a Path Alignment critic will do you well. Not over-weighting the path align critic will allow the robot to  deviate from the path to get around dynamic obstacles in the scene or other obstacles not previous considered during path planning. It is subjective as to the best behavior for your application, but it has been shown that MPPI can be an exact path tracker and/or avoid dynamic obstacles very fluidly and everywhere in between. The defaults provided are in the generally right regime for a balanced initial trade-off.
 
 ### MFMA and AVX2 Optimizations
 
-This MPPI is made possible to run on CPU-only by using a very well optimized implementation that rely on CPU vectorization through AVX2 and MFMA. All even remotely modern computers support this (2013+), but if using a very old computer you may not be able to use the plugin. Note that MPC is computationally heavy to begin with, so computers circa-2013 even if it were to have those compiler flags available probably wouldn't run it at a satisfactory rate anyway. 
+This MPPI is made possible to run on CPU-only by using a very well optimized implementation that rely on CPU vectorization through AVX2 and MFMA. All even remotely modern computers support this (2013+), but if using a very old computer you may not be able to use the plugin. Note that MPC is computationally heavy to begin with, so computers circa-2013 even if it were to have those compiler flags available probably wouldn't run it at a satisfactory rate anyway.
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp
index 0816fcc32e6..c2fc9b5cc9e 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp
@@ -105,16 +105,20 @@ class MPPIController : public nav2_core::Controller
   /**
     * @brief Visualize trajectories
     * @param transformed_plan Transformed input plan
+    * @param cmd_stamp Command stamp
+    * @param optimal_trajectory Optimal trajectory, if already computed
     */
   void visualize(
     nav_msgs::msg::Path transformed_plan,
-    const builtin_interfaces::msg::Time & cmd_stamp);
+    const builtin_interfaces::msg::Time & cmd_stamp,
+    const Eigen::ArrayXXf & optimal_trajectory);
 
   std::string name_;
   rclcpp_lifecycle::LifecycleNode::WeakPtr parent_;
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::Trajectory>::SharedPtr opt_traj_pub_;
 
   std::unique_ptr<ParametersHandler> parameters_handler_;
   Optimizer optimizer_;
@@ -122,6 +126,7 @@ class MPPIController : public nav2_core::Controller
   TrajectoryVisualizer trajectory_visualizer_;
 
   bool visualize_;
+  bool publish_optimal_trajectory_;
 };
 
 }  // namespace nav2_mppi_controller
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
index f9f1f8d7939..c285a14b664 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
@@ -133,6 +133,7 @@ class CostCritic : public CriticFunction
   float circumscribed_cost_{0.0f};
   float collision_cost_{0.0f};
   float critical_cost_{0.0f};
+  unsigned int near_collision_cost_{253};
   float weight_{0};
   unsigned int trajectory_point_step_;
 
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp
index 0ae35f0d050..bd8b972473d 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp
@@ -30,6 +30,7 @@ struct ControlConstraints
   float wz;
   float ax_max;
   float ax_min;
+  float ay_min;
   float ay_max;
   float az_max;
 };
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp
index 459da412c63..9416b53e1ba 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp
@@ -27,8 +27,8 @@ namespace mppi::models
  */
 struct OptimizerSettings
 {
-  models::ControlConstraints base_constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
-  models::ControlConstraints constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+  models::ControlConstraints base_constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+  models::ControlConstraints constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
   models::SamplingStd sampling_std{0.0f, 0.0f, 0.0f};
   float model_dt{0.0f};
   float temperature{0.0f};
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp
index eeeec735e7b..17fce5ac4b9 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp
@@ -1,4 +1,5 @@
 // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2025 Open Navigation LLC
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -75,14 +76,24 @@ class MotionModel
     float max_delta_vx = model_dt_ * control_constraints_.ax_max;
     float min_delta_vx = model_dt_ * control_constraints_.ax_min;
     float max_delta_vy = model_dt_ * control_constraints_.ay_max;
+    float min_delta_vy = model_dt_ * control_constraints_.ay_min;
     float max_delta_wz = model_dt_ * control_constraints_.az_max;
 
     unsigned int n_cols = state.vx.cols();
 
     for (unsigned int i = 1; i < n_cols; i++) {
+      auto lower_bound_vx = (state.vx.col(i - 1) >
+        0).select(
+        state.vx.col(i - 1) + min_delta_vx,
+        state.vx.col(i - 1) - max_delta_vx);
+      auto upper_bound_vx = (state.vx.col(i - 1) >
+        0).select(
+        state.vx.col(i - 1) + max_delta_vx,
+        state.vx.col(i - 1) - min_delta_vx);
+
       state.cvx.col(i - 1) = state.cvx.col(i - 1)
-        .cwiseMax(state.vx.col(i - 1) + min_delta_vx)
-        .cwiseMin(state.vx.col(i - 1) + max_delta_vx);
+        .cwiseMax(lower_bound_vx)
+        .cwiseMin(upper_bound_vx);
       state.vx.col(i) = state.cvx.col(i - 1);
 
       state.cwz.col(i - 1) = state.cwz.col(i - 1)
@@ -91,9 +102,17 @@ class MotionModel
       state.wz.col(i) = state.cwz.col(i - 1);
 
       if (is_holo) {
+        auto lower_bound_vy = (state.vy.col(i - 1) >
+          0).select(
+          state.vy.col(i - 1) + min_delta_vy,
+          state.vy.col(i - 1) - max_delta_vy);
+        auto upper_bound_vy = (state.vy.col(i - 1) >
+          0).select(
+          state.vy.col(i - 1) + max_delta_vy,
+          state.vy.col(i - 1) - min_delta_vy);
         state.cvy.col(i - 1) = state.cvy.col(i - 1)
-          .cwiseMax(state.vy.col(i - 1) - max_delta_vy)
-          .cwiseMin(state.vy.col(i - 1) + max_delta_vy);
+          .cwiseMax(lower_bound_vy)
+          .cwiseMin(upper_bound_vy);
         state.vy.col(i) = state.cvy.col(i - 1);
       }
     }
@@ -114,7 +133,7 @@ class MotionModel
 protected:
   float model_dt_{0.0};
   models::ControlConstraints control_constraints_{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
-    0.0f};
+    0.0f, 0.0f};
 };
 
 /**
@@ -151,7 +170,7 @@ class AckermannMotionModel : public MotionModel
     const auto vx_ptr = control_sequence.vx.data();
     auto wz_ptr = control_sequence.wz.data();
     int steps = control_sequence.vx.size();
-    for(int i = 0; i < steps; i++) {
+    for (int i = 0; i < steps; i++) {
       float wz_constrained = fabs(*(vx_ptr + i) / min_turning_r_);
       float & wz_curr = *(wz_ptr + i);
       wz_curr = utils::clamp(-1 * wz_constrained, wz_constrained, wz_curr);
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
index c84ef98e946..e0821e548d6 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
@@ -105,6 +105,12 @@ class Optimizer
    */
   Eigen::ArrayXXf getOptimizedTrajectory();
 
+  /**
+   * @brief Get the optimal control sequence for a cycle for visualization
+   * @return Optimal control sequence
+   */
+  const models::ControlSequence & getOptimalControlSequence();
+
   /**
    * @brief Set the maximum speed based on the speed limits callback
    * @param speed_limit Limit of the speed for use
@@ -117,6 +123,15 @@ class Optimizer
    */
   void reset();
 
+  /**
+   * @brief Get the motion model time step
+   * @return Time step of the model
+   */
+  const models::OptimizerSettings & getSettings() const
+  {
+    return settings_;
+  }
+
 protected:
   /**
    * @brief Main function to generate, score, and return trajectories
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
index 5e52d6468e7..2ca44fd0137 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
@@ -32,6 +32,7 @@
 
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav_msgs/msg/path.hpp"
+#include "nav2_msgs/msg/trajectory.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
 
 #include "rclcpp/rclcpp.hpp"
@@ -170,6 +171,34 @@ inline geometry_msgs::msg::TwistStamped toTwistStamped(
   return twist;
 }
 
+inline std::unique_ptr<nav2_msgs::msg::Trajectory> toTrajectoryMsg(
+  const Eigen::ArrayXXf & trajectory,
+  const models::ControlSequence & control_sequence,
+  const double & model_dt,
+  const std_msgs::msg::Header & header)
+{
+  auto trajectory_msg = std::make_unique<nav2_msgs::msg::Trajectory>();
+  trajectory_msg->header = header;
+  trajectory_msg->points.resize(trajectory.rows());
+
+  for (int i = 0; i < trajectory.rows(); ++i) {
+    auto & curr_pt = trajectory_msg->points[i];
+    curr_pt.time_from_start = rclcpp::Duration::from_seconds(i * model_dt);
+    curr_pt.pose.position.x = trajectory(i, 0);
+    curr_pt.pose.position.y = trajectory(i, 1);
+    tf2::Quaternion quat;
+    quat.setRPY(0.0, 0.0, trajectory(i, 2));
+    curr_pt.pose.orientation = tf2::toMsg(quat);
+    curr_pt.velocity.linear.x = control_sequence.vx(i);
+    curr_pt.velocity.angular.z = control_sequence.wz(i);
+    if (control_sequence.vy.size() > 0) {
+      curr_pt.velocity.linear.y = control_sequence.vy(i);
+    }
+  }
+
+  return trajectory_msg;
+}
+
 /**
  * @brief Convert path to a tensor
  * @param path Path to convert
@@ -256,10 +285,11 @@ inline bool withinPositionGoalTolerance(
 template<typename T>
 auto normalize_angles(const T & angles)
 {
-  return (angles + M_PIF).unaryExpr([&](const float x) {
-             float remainder = std::fmod(x, 2.0f * M_PIF);
-             return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF;
-             });
+  return (angles + M_PIF).unaryExpr(
+    [&](const float x) {
+      float remainder = std::fmod(x, 2.0f * M_PIF);
+      return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF;
+    });
 }
 
 /**
@@ -458,7 +488,7 @@ inline void savitskyGolayFilter(
 
   auto applyFilterOverAxis =
     [&](Eigen::ArrayXf & sequence, const Eigen::ArrayXf & initial_sequence,
-    const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void
+      const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void
     {
       float pt_m4 = hist_0;
       float pt_m3 = hist_1;
@@ -607,15 +637,15 @@ struct Pose2D
 inline void shiftColumnsByOnePlace(Eigen::Ref<Eigen::ArrayXXf> e, int direction)
 {
   int size = e.size();
-  if(size == 1) {return;}
-  if(abs(direction) != 1) {
+  if (size == 1) {return;}
+  if (abs(direction) != 1) {
     throw std::logic_error("Invalid direction, only 1 and -1 are valid values.");
   }
 
-  if((e.cols() == 1 || e.rows() == 1) && size > 1) {
+  if ((e.cols() == 1 || e.rows() == 1) && size > 1) {
     auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1;
     auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1;
-    while(start_ptr != end_ptr) {
+    while (start_ptr != end_ptr) {
       *(start_ptr + direction) = *start_ptr;
       start_ptr -= direction;
     }
@@ -624,7 +654,7 @@ inline void shiftColumnsByOnePlace(Eigen::Ref<Eigen::ArrayXXf> e, int direction)
     auto start_ptr = direction == 1 ? e.data() + size - 2 * e.rows() : e.data() + e.rows();
     auto end_ptr = direction == 1 ? e.data() : e.data() + size - e.rows();
     auto span = e.rows();
-    while(start_ptr != end_ptr) {
+    while (start_ptr != end_ptr) {
       std::copy(start_ptr, start_ptr + span, start_ptr + direction * span);
       start_ptr -= (direction * span);
     }
@@ -644,10 +674,10 @@ inline auto normalize_yaws_between_points(
   const Eigen::Ref<const Eigen::ArrayXf> & yaw_between_points)
 {
   Eigen::ArrayXf yaws = utils::shortest_angular_distance(
-          last_yaws, yaw_between_points).abs();
+    last_yaws, yaw_between_points).abs();
   int size = yaws.size();
   Eigen::ArrayXf yaws_between_points_corrected(size);
-  for(int i = 0; i != size; i++) {
+  for (int i = 0; i != size; i++) {
     const float & yaw_between_point = yaw_between_points[i];
     yaws_between_points_corrected[i] = yaws[i] < M_PIF_2 ?
       yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF);
@@ -666,7 +696,7 @@ inline auto normalize_yaws_between_points(
 {
   int size = yaw_between_points.size();
   Eigen::ArrayXf yaws_between_points_corrected(size);
-  for(int i = 0; i != size; i++) {
+  for (int i = 0; i != size; i++) {
     const float & yaw_between_point = yaw_between_points[i];
     yaws_between_points_corrected[i] = fabs(
       angles::normalize_angle(yaw_between_point - goal_yaw)) < M_PIF_2 ?
diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp
index c9ff6e9e0f0..57effc952ee 100644
--- a/nav2_mppi_controller/src/controller.cpp
+++ b/nav2_mppi_controller/src/controller.cpp
@@ -38,6 +38,8 @@ void MPPIController::configure(
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(visualize_, "visualize", false);
 
+  getParam(publish_optimal_trajectory_, "publish_optimal_trajectory", false);
+
   // Configure composed objects
   optimizer_.initialize(parent_, name_, costmap_ros_, parameters_handler_.get());
   path_handler_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get());
@@ -45,6 +47,11 @@ void MPPIController::configure(
     parent_, name_,
     costmap_ros_->getGlobalFrameID(), parameters_handler_.get());
 
+  if (publish_optimal_trajectory_) {
+    opt_traj_pub_ = node->create_publisher<nav2_msgs::msg::Trajectory>(
+      "~/optimal_trajectory", rclcpp::SystemDefaultsQoS());
+  }
+
   RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str());
 }
 
@@ -53,19 +60,27 @@ void MPPIController::cleanup()
   optimizer_.shutdown();
   trajectory_visualizer_.on_cleanup();
   parameters_handler_.reset();
+  opt_traj_pub_.reset();
   RCLCPP_INFO(logger_, "Cleaned up MPPI Controller: %s", name_.c_str());
 }
 
 void MPPIController::activate()
 {
+  auto node = parent_.lock();
   trajectory_visualizer_.on_activate();
   parameters_handler_->start();
+  if (opt_traj_pub_) {
+    opt_traj_pub_->on_activate();
+  }
   RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str());
 }
 
 void MPPIController::deactivate()
 {
   trajectory_visualizer_.on_deactivate();
+  if (opt_traj_pub_) {
+    opt_traj_pub_->on_deactivate();
+  }
   RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str());
 }
 
@@ -100,8 +115,19 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
   RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration);
 #endif
 
+  Eigen::ArrayXXf optimal_trajectory;
+  if (publish_optimal_trajectory_ && opt_traj_pub_->get_subscription_count() > 0) {
+    optimal_trajectory = optimizer_.getOptimizedTrajectory();
+    auto trajectory_msg = utils::toTrajectoryMsg(
+      optimal_trajectory,
+      optimizer_.getOptimalControlSequence(),
+      optimizer_.getSettings().model_dt,
+      cmd.header);
+    opt_traj_pub_->publish(std::move(trajectory_msg));
+  }
+
   if (visualize_) {
-    visualize(std::move(transformed_plan), cmd.header.stamp);
+    visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory);
   }
 
   return cmd;
@@ -109,10 +135,16 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
 
 void MPPIController::visualize(
   nav_msgs::msg::Path transformed_plan,
-  const builtin_interfaces::msg::Time & cmd_stamp)
+  const builtin_interfaces::msg::Time & cmd_stamp,
+  const Eigen::ArrayXXf & optimal_trajectory)
 {
   trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
-  trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory", cmd_stamp);
+  if (optimal_trajectory.size() > 0) {
+    trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp);
+  } else {
+    trajectory_visualizer_.add(
+      optimizer_.getOptimizedTrajectory(), "Optimal Trajectory", cmd_stamp);
+  }
   trajectory_visualizer_.visualize(std::move(transformed_plan));
 }
 
diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp
index bce8bdc7fca..4ed43dbf5e8 100644
--- a/nav2_mppi_controller/src/critics/constraint_critic.cpp
+++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp
@@ -64,7 +64,7 @@ void ConstraintCritic::score(CriticData & data)
     unsigned int n_rows = data.state.vx.rows();
     unsigned int n_cols = data.state.vx.cols();
     Eigen::ArrayXXf sgn(n_rows, n_cols);
-    sgn = vx.unaryExpr([](const float x){return copysignf(1.0f, x);});
+    sgn = vx.unaryExpr([](const float x) {return copysignf(1.0f, x);});
 
     auto vel_total = sgn * (data.state.vx.square() + data.state.vy.square()).sqrt();
     if (power_ > 1u) {
diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp
index 730f13423aa..3b84384c8b9 100644
--- a/nav2_mppi_controller/src/critics/cost_critic.cpp
+++ b/nav2_mppi_controller/src/critics/cost_critic.cpp
@@ -15,6 +15,7 @@
 
 #include <cmath>
 #include "nav2_mppi_controller/critics/cost_critic.hpp"
+#include "nav2_core/controller_exceptions.hpp"
 
 namespace mppi::critics
 {
@@ -26,6 +27,7 @@ void CostCritic::initialize()
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 3.81f);
   getParam(critical_cost_, "critical_cost", 300.0f);
+  getParam(near_collision_cost_, "near_collision_cost", 253);
   getParam(collision_cost_, "collision_cost", 1000000.0f);
   getParam(near_goal_distance_, "near_goal_distance", 0.5f);
   getParam(inflation_layer_name_, "inflation_layer_name", std::string(""));
@@ -54,6 +56,22 @@ void CostCritic::initialize()
       " for full instructions. This will substantially impact run-time performance.");
   }
 
+  if (costmap_ros_->getUseRadius() == consider_footprint_) {
+    RCLCPP_WARN(
+      logger_,
+      "Inconsistent configuration in collision checking. Please verify the robot's shape settings "
+      "in both the costmap and the cost critic.");
+    if (costmap_ros_->getUseRadius()) {
+      throw nav2_core::ControllerException(
+              "Considering footprint in collision checking but no robot footprint provided in the "
+              "costmap.");
+    }
+  }
+
+  if (near_collision_cost_ > 253) {
+    RCLCPP_WARN(logger_, "Near collision cost is set higher than INSCRIBED_INFLATED_OBSTACLE");
+  }
+
   RCLCPP_INFO(
     logger_,
     "InflationCostCritic instantiated with %d power and %f / %f weights. "
@@ -130,14 +148,17 @@ void CostCritic::score(CriticData & data)
   int outer_stride = strided_traj_rows * trajectory_point_step_;
 
   const auto traj_x = Eigen::Map<const Eigen::ArrayXXf, 0,
-      Eigen::Stride<-1, -1>>(data.trajectories.x.data(), strided_traj_rows, strided_traj_cols,
-      Eigen::Stride<-1, -1>(outer_stride, 1));
+      Eigen::Stride<-1, -1>>(
+    data.trajectories.x.data(), strided_traj_rows, strided_traj_cols,
+    Eigen::Stride<-1, -1>(outer_stride, 1));
   const auto traj_y = Eigen::Map<const Eigen::ArrayXXf, 0,
-      Eigen::Stride<-1, -1>>(data.trajectories.y.data(), strided_traj_rows, strided_traj_cols,
-      Eigen::Stride<-1, -1>(outer_stride, 1));
+      Eigen::Stride<-1, -1>>(
+    data.trajectories.y.data(), strided_traj_rows, strided_traj_cols,
+    Eigen::Stride<-1, -1>(outer_stride, 1));
   const auto traj_yaw = Eigen::Map<const Eigen::ArrayXXf, 0,
-      Eigen::Stride<-1, -1>>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols,
-      Eigen::Stride<-1, -1>(outer_stride, 1));
+      Eigen::Stride<-1, -1>>(
+    data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols,
+    Eigen::Stride<-1, -1>(outer_stride, 1));
 
   for (int i = 0; i < strided_traj_rows; ++i) {
     bool trajectory_collide = false;
@@ -153,28 +174,24 @@ void CostCritic::score(CriticData & data)
       // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it
       // So the center point has more information than the footprint
       if (!worldToMapFloat(Tx, Ty, x_i, y_i)) {
-        if (!is_tracking_unknown_) {
-          traj_cost = collision_cost_;
-          trajectory_collide = true;
-          break;
-        }
         pose_cost = 255.0f;  // NO_INFORMATION in float
       } else {
         pose_cost = static_cast<float>(costmap->getCost(getIndex(x_i, y_i)));
         if (pose_cost < 1.0f) {
           continue;  // In free space
         }
-        if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) {
-          traj_cost = collision_cost_;
-          trajectory_collide = true;
-          break;
-        }
+      }
+
+      if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) {
+        traj_cost = collision_cost_;
+        trajectory_collide = true;
+        break;
       }
 
       // Let near-collision trajectory points be punished severely
       // Note that we collision check based on the footprint actual,
       // but score based on the center-point cost regardless
-      if (pose_cost >= 253.0f /*INSCRIBED_INFLATED_OBSTACLE in float*/) {
+      if (pose_cost >= static_cast<float>(near_collision_cost_)) {
         traj_cost += critical_cost_;
       } else if (!near_goal) {  // Generally prefer trajectories further from obstacles
         traj_cost += pose_cost;
diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp
index 6bb615805d2..aada6ff4920 100644
--- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp
+++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp
@@ -44,7 +44,7 @@ void GoalAngleCritic::score(CriticData & data)
   const auto goal_idx = data.path.x.size() - 1;
   const float goal_yaw = data.path.yaws(goal_idx);
 
-  if(power_ > 1u) {
+  if (power_ > 1u) {
     data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()).
       rowwise().mean()) * weight_).pow(power_).eval();
   } else {
diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp
index 93052b5adec..282ddc62850 100644
--- a/nav2_mppi_controller/src/critics/goal_critic.cpp
+++ b/nav2_mppi_controller/src/critics/goal_critic.cpp
@@ -46,7 +46,7 @@ void GoalCritic::score(CriticData & data)
   const auto delta_x = data.trajectories.x - goal_x;
   const auto delta_y = data.trajectories.y - goal_y;
 
-  if(power_ > 1u) {
+  if (power_ > 1u) {
     data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean() *
       weight_).pow(power_);
   } else {
diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp
index 5d05dcd3e69..5f115b5e205 100644
--- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp
+++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp
@@ -16,6 +16,8 @@
 #include <cmath>
 #include "nav2_mppi_controller/critics/obstacles_critic.hpp"
 #include "nav2_costmap_2d/inflation_layer.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+
 namespace mppi::critics
 {
 
@@ -44,6 +46,18 @@ void ObstaclesCritic::initialize()
       " for full instructions. This will substantially impact run-time performance.");
   }
 
+  if (costmap_ros_->getUseRadius() == consider_footprint_) {
+    RCLCPP_WARN(
+      logger_,
+      "Inconsistent configuration in collision checking. Please verify the robot's shape settings "
+      "in both the costmap and the obstacle critic.");
+    if (costmap_ros_->getUseRadius()) {
+      throw nav2_core::ControllerException(
+              "Considering footprint in collision checking but no robot footprint provided in the "
+              "costmap.");
+    }
+  }
+
   RCLCPP_INFO(
     logger_,
     "ObstaclesCritic instantiated with %d power and %f / %f weights. "
@@ -126,7 +140,7 @@ void ObstaclesCritic::score(CriticData & data)
   const unsigned int batch_size = data.trajectories.x.rows();
   bool all_trajectories_collide = true;
 
-  for(unsigned int i = 0; i != batch_size; i++) {
+  for (unsigned int i = 0; i != batch_size; i++) {
     bool trajectory_collide = false;
     float traj_cost = 0.0f;
     const auto & traj = data.trajectories;
@@ -134,7 +148,7 @@ void ObstaclesCritic::score(CriticData & data)
     raw_cost(i) = 0.0f;
     repulsive_cost(i) = 0.0f;
 
-    for(unsigned int j = 0; j != traj_len; j++) {
+    for (unsigned int j = 0; j != traj_len; j++) {
       pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j));
       if (pose_cost.cost < 1.0f) {continue;}  // In free space
 
diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp
index ac0e4b0b0ea..71b05e0a7da 100644
--- a/nav2_mppi_controller/src/critics/path_align_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp
@@ -101,14 +101,17 @@ void PathAlignCritic::score(CriticData & data)
   int outer_stride = strided_traj_rows * trajectory_point_step_;
   // Get strided trajectory information
   const auto T_x = Eigen::Map<const Eigen::ArrayXXf, 0,
-      Eigen::Stride<-1, -1>>(data.trajectories.x.data(),
-      strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1));
+      Eigen::Stride<-1, -1>>(
+    data.trajectories.x.data(),
+    strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1));
   const auto T_y = Eigen::Map<const Eigen::ArrayXXf, 0,
-      Eigen::Stride<-1, -1>>(data.trajectories.y.data(),
-      strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1));
+      Eigen::Stride<-1, -1>>(
+    data.trajectories.y.data(),
+    strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1));
   const auto T_yaw = Eigen::Map<const Eigen::ArrayXXf, 0,
-      Eigen::Stride<-1, -1>>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols,
-      Eigen::Stride<-1, -1>(outer_stride, 1));
+      Eigen::Stride<-1, -1>>(
+    data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols,
+    Eigen::Stride<-1, -1>(outer_stride, 1));
   const auto traj_sampled_size = T_x.cols();
 
   for (size_t t = 0; t < batch_size; ++t) {
diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp
index 7888cbb1488..a312b1e50ae 100644
--- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp
@@ -70,7 +70,7 @@ void PathAngleCritic::score(CriticData & data)
   utils::setPathFurthestPointIfNotSet(data);
   auto offsetted_idx = std::min(
     *data.furthest_reached_path_point + offset_from_furthest_,
-      static_cast<size_t>(data.path.x.size()) - 1);
+    static_cast<size_t>(data.path.x.size()) - 1);
 
   const float goal_x = data.path.x(offsetted_idx);
   const float goal_y = data.path.y(offsetted_idx);
@@ -101,7 +101,7 @@ void PathAngleCritic::score(CriticData & data)
   auto diff_y = goal_y - data.trajectories.y.col(last_idx);
   auto diff_x = goal_x - data.trajectories.x.col(last_idx);
   auto yaws_between_points = diff_y.binaryExpr(
-    diff_x, [&](const float & y, const float & x){return atan2f(y, x);}).eval();
+    diff_x, [&](const float & y, const float & x) {return atan2f(y, x);}).eval();
 
   switch (mode_) {
     case PathAngleMode::FORWARD_PREFERENCE:
@@ -119,7 +119,8 @@ void PathAngleCritic::score(CriticData & data)
     case PathAngleMode::NO_DIRECTIONAL_PREFERENCE:
       {
         auto last_yaws = data.trajectories.yaws.col(last_idx);
-        auto yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws,
+        auto yaws_between_points_corrected = utils::normalize_yaws_between_points(
+          last_yaws,
           yaws_between_points);
         auto corrected_yaws = utils::shortest_angular_distance(
           last_yaws, yaws_between_points_corrected).abs();
@@ -133,7 +134,8 @@ void PathAngleCritic::score(CriticData & data)
     case PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS:
       {
         auto last_yaws = data.trajectories.yaws.col(last_idx);
-        auto yaws_between_points_corrected = utils::normalize_yaws_between_points(goal_yaw,
+        auto yaws_between_points_corrected = utils::normalize_yaws_between_points(
+          goal_yaw,
           yaws_between_points);
         auto corrected_yaws = utils::shortest_angular_distance(
           last_yaws, yaws_between_points_corrected).abs();
diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp
index 4d407414f76..b5fb3880c14 100644
--- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp
+++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp
@@ -42,10 +42,10 @@ void PreferForwardCritic::score(CriticData & data)
 
   if (power_ > 1u) {
     data.costs += (
-      (data.state.vx.unaryExpr([&](const float & x){return std::max(-x, 0.0f);}) *
+      (data.state.vx.unaryExpr([&](const float & x) {return std::max(-x, 0.0f);}) *
       data.model_dt).rowwise().sum() * weight_).pow(power_);
   } else {
-    data.costs += (data.state.vx.unaryExpr([&](const float & x){return std::max(-x, 0.0f);}) *
+    data.costs += (data.state.vx.unaryExpr([&](const float & x) {return std::max(-x, 0.0f);}) *
       data.model_dt).rowwise().sum() * weight_;
   }
 }
diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp
index a704ef8b5cf..eea580fe569 100644
--- a/nav2_mppi_controller/src/noise_generator.cpp
+++ b/nav2_mppi_controller/src/noise_generator.cpp
@@ -109,12 +109,12 @@ void NoiseGenerator::generateNoisedControls()
 {
   auto & s = settings_;
   noises_vx_ = Eigen::ArrayXXf::NullaryExpr(
-    s.batch_size, s.time_steps, [&] () {return ndistribution_vx_(generator_);});
+    s.batch_size, s.time_steps, [&]() {return ndistribution_vx_(generator_);});
   noises_wz_ = Eigen::ArrayXXf::NullaryExpr(
-    s.batch_size, s.time_steps, [&] () {return ndistribution_wz_(generator_);});
-  if(is_holonomic_) {
+    s.batch_size, s.time_steps, [&]() {return ndistribution_wz_(generator_);});
+  if (is_holonomic_) {
     noises_vy_ = Eigen::ArrayXXf::NullaryExpr(
-      s.batch_size, s.time_steps, [&] () {return ndistribution_vy_(generator_);});
+      s.batch_size, s.time_steps, [&]() {return ndistribution_vy_(generator_);});
   }
 }
 
diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
index a381685c6d6..6bd0d9c4ff0 100644
--- a/nav2_mppi_controller/src/optimizer.cpp
+++ b/nav2_mppi_controller/src/optimizer.cpp
@@ -1,4 +1,5 @@
 // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2025 Open Navigation LLC
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -75,6 +76,7 @@ void Optimizer::getParams()
   getParam(s.base_constraints.ax_max, "ax_max", 3.0f);
   getParam(s.base_constraints.ax_min, "ax_min", -3.0f);
   getParam(s.base_constraints.ay_max, "ay_max", 3.0f);
+  getParam(s.base_constraints.ay_min, "ay_min", -3.0f);
   getParam(s.base_constraints.az_max, "az_max", 3.5f);
   getParam(s.sampling_std.vx, "vx_std", 0.2f);
   getParam(s.sampling_std.vy, "vy_std", 0.2f);
@@ -89,6 +91,14 @@ void Optimizer::getParams()
       "Sign of the parameter ax_min is incorrect, consider setting it negative.");
   }
 
+  if (s.base_constraints.ay_min > 0.0) {
+    s.base_constraints.ay_min = -1.0 * s.base_constraints.ay_min;
+    RCLCPP_WARN(
+      logger_,
+      "Sign of the parameter ay_min is incorrect, consider setting it negative.");
+  }
+
+
   getParam(motion_model_name, "motion_model", std::string("DiffDrive"));
 
   s.constraints = s.base_constraints;
@@ -245,6 +255,7 @@ void Optimizer::applyControlSequenceConstraints()
   float max_delta_vx = s.model_dt * s.constraints.ax_max;
   float min_delta_vx = s.model_dt * s.constraints.ax_min;
   float max_delta_vy = s.model_dt * s.constraints.ay_max;
+  float min_delta_vy = s.model_dt * s.constraints.ay_min;
   float max_delta_wz = s.model_dt * s.constraints.az_max;
   float vx_last = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, control_sequence_.vx(0));
   float wz_last = utils::clamp(-s.constraints.wz, s.constraints.wz, control_sequence_.wz(0));
@@ -259,7 +270,11 @@ void Optimizer::applyControlSequenceConstraints()
   for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) {
     float & vx_curr = control_sequence_.vx(i);
     vx_curr = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, vx_curr);
-    vx_curr = utils::clamp(vx_last + min_delta_vx, vx_last + max_delta_vx, vx_curr);
+    if (vx_last > 0) {
+      vx_curr = utils::clamp(vx_last + min_delta_vx, vx_last + max_delta_vx, vx_curr);
+    } else {
+      vx_curr = utils::clamp(vx_last - max_delta_vx, vx_last - min_delta_vx, vx_curr);
+    }
     vx_last = vx_curr;
 
     float & wz_curr = control_sequence_.wz(i);
@@ -270,7 +285,11 @@ void Optimizer::applyControlSequenceConstraints()
     if (isHolonomic()) {
       float & vy_curr = control_sequence_.vy(i);
       vy_curr = utils::clamp(-s.constraints.vy, s.constraints.vy, vy_curr);
-      vy_curr = utils::clamp(vy_last - max_delta_vy, vy_last + max_delta_vy, vy_curr);
+      if (vy_last > 0) {
+        vy_curr = utils::clamp(vy_last + min_delta_vy, vy_last + max_delta_vy, vy_curr);
+      } else {
+        vy_curr = utils::clamp(vy_last - max_delta_vy, vy_last - min_delta_vy, vy_curr);
+      }
       vy_last = vy_curr;
     }
   }
@@ -318,7 +337,7 @@ void Optimizer::integrateStateVelocities(
   const size_t n_size = traj_yaws.size();
 
   float last_yaw = initial_yaw;
-  for(size_t i = 0; i != n_size; i++) {
+  for (size_t i = 0; i != n_size; i++) {
     last_yaw += wz(i) * settings_.model_dt;
     traj_yaws(i) = last_yaw;
   }
@@ -341,7 +360,7 @@ void Optimizer::integrateStateVelocities(
 
   float last_x = state_.pose.pose.position.x;
   float last_y = state_.pose.pose.position.y;
-  for(size_t i = 0; i != n_size; i++) {
+  for (size_t i = 0; i != n_size; i++) {
     last_x += dx(i) * settings_.model_dt;
     last_y += dy(i) * settings_.model_dt;
     traj_x(i) = last_x;
@@ -377,9 +396,11 @@ void Optimizer::integrateStateVelocities(
     dy += state.vy * yaw_cos;
   }
 
-  Eigen::ArrayXf last_x = Eigen::ArrayXf::Constant(trajectories.x.rows(),
+  Eigen::ArrayXf last_x = Eigen::ArrayXf::Constant(
+    trajectories.x.rows(),
     state.pose.pose.position.x);
-  Eigen::ArrayXf last_y = Eigen::ArrayXf::Constant(trajectories.y.rows(),
+  Eigen::ArrayXf last_y = Eigen::ArrayXf::Constant(
+    trajectories.y.rows(),
     state.pose.pose.position.y);
 
   for (size_t i = 0; i != n_cols; i++) {
@@ -408,6 +429,11 @@ Eigen::ArrayXXf Optimizer::getOptimizedTrajectory()
   return trajectories;
 }
 
+const models::ControlSequence & Optimizer::getOptimalControlSequence()
+{
+  return control_sequence_;
+}
+
 void Optimizer::updateControlSequence()
 {
   const bool is_holo = isHolonomic();
diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp
index 0a6b2a4d47e..de4f2249fb9 100644
--- a/nav2_mppi_controller/src/trajectory_visualizer.cpp
+++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp
@@ -26,9 +26,10 @@ void TrajectoryVisualizer::on_configure(
   logger_ = node->get_logger();
   frame_id_ = frame_id;
   trajectories_publisher_ =
-    node->create_publisher<visualization_msgs::msg::MarkerArray>("trajectories", 1);
-  transformed_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("transformed_global_plan", 1);
-  optimal_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("optimal_trajectory", 1);
+    node->create_publisher<visualization_msgs::msg::MarkerArray>("~/candidate_trajectories", 1);
+  transformed_path_pub_ = node->create_publisher<nav_msgs::msg::Path>(
+    "~/transformed_global_plan", 1);
+  optimal_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("~/optimal_path", 1);
   parameters_handler_ = parameters_handler;
 
   auto getParam = parameters_handler->getParamGetter(name + ".TrajectoryVisualizer");
diff --git a/nav2_mppi_controller/test/controller_state_transition_test.cpp b/nav2_mppi_controller/test/controller_state_transition_test.cpp
index 5cb66a27aff..5bdfb8a1121 100644
--- a/nav2_mppi_controller/test/controller_state_transition_test.cpp
+++ b/nav2_mppi_controller/test/controller_state_transition_test.cpp
@@ -38,6 +38,7 @@ TEST(ControllerStateTransitionTest, ControllerNotFail)
   options.parameter_overrides(params);
 
   auto node = getDummyNode(options);
+  node->declare_parameter("publish_optimal_trajectory", true);
   auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock());
   auto costmap_ros = getDummyCostmapRos(costmap_settings);
   costmap_ros->setRobotFootprint(getDummySquareFootprint(0.01));
@@ -66,7 +67,7 @@ TEST(ControllerStateTransitionTest, ControllerNotFail)
   controller->cleanup();
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_mppi_controller/test/critic_manager_test.cpp b/nav2_mppi_controller/test/critic_manager_test.cpp
index 455301eeda3..0c83a492e63 100644
--- a/nav2_mppi_controller/test/critic_manager_test.cpp
+++ b/nav2_mppi_controller/test/critic_manager_test.cpp
@@ -144,7 +144,7 @@ TEST(CriticManagerTests, CriticLoadingTest)
   EXPECT_EQ(critic_manager.getCriticNum(), 2u);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp
index e1934affeec..0751d1279c8 100644
--- a/nav2_mppi_controller/test/critics_tests.cpp
+++ b/nav2_mppi_controller/test/critics_tests.cpp
@@ -126,6 +126,81 @@ TEST(CriticTests, ConstraintsCritic)
   EXPECT_NEAR(costs(1), 0.48, 0.01);
 }
 
+TEST(CriticTests, ObstacleCriticMisalignedParams) {
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  ParametersHandler param_handler(node);
+  auto getParam = param_handler.getParamGetter("critic");
+  bool consider_footprint;
+  getParam(consider_footprint, "consider_footprint", true);
+
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  ObstaclesCritic critic;
+  // Expect throw when settings mismatched
+  EXPECT_THROW(
+    critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler),
+    nav2_core::ControllerException
+  );
+}
+
+TEST(CriticTests, ObstacleCriticAlignedParams) {
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  ParametersHandler param_handler(node);
+  auto getParam = param_handler.getParamGetter("critic");
+  bool consider_footprint;
+  getParam(consider_footprint, "consider_footprint", false);
+
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  ObstaclesCritic critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+}
+
+
+TEST(CriticTests, CostCriticMisAlignedParams) {
+  // Standard preamble
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  ParametersHandler param_handler(node);
+  rclcpp_lifecycle::State lstate;
+  auto getParam = param_handler.getParamGetter("critic");
+  bool consider_footprint;
+  getParam(consider_footprint, "consider_footprint", true);
+  costmap_ros->on_configure(lstate);
+
+  CostCritic critic;
+  // Expect throw when settings mismatched
+  EXPECT_THROW(
+    critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler),
+    nav2_core::ControllerException
+  );
+}
+
+TEST(CriticTests, CostCriticAlignedParams) {
+  // Standard preamble
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  ParametersHandler param_handler(node);
+  rclcpp_lifecycle::State lstate;
+  auto getParam = param_handler.getParamGetter("critic");
+  bool consider_footprint;
+  getParam(consider_footprint, "consider_footprint", false);
+  costmap_ros->on_configure(lstate);
+
+  CostCritic critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+}
+
 TEST(CriticTests, GoalAngleCritic)
 {
   // Standard preamble
@@ -168,7 +243,7 @@ TEST(CriticTests, GoalAngleCritic)
   critic.score(data);
   EXPECT_NEAR(costs.sum(), 0, 1e-6);
 
-  // Lets move it even closer, just to be sure it still doesn't trigger
+  // Let's move it even closer, just to be sure it still doesn't trigger
   state.pose.pose.position.x = 9.2;
   critic.score(data);
   EXPECT_NEAR(costs.sum(), 0, 1e-6);
@@ -465,7 +540,7 @@ TEST(CriticTests, TwirlingCritic)
   // Now try again with some wiggling noise
   std::mt19937 engine;
   std::normal_distribution<float> normal_dist = std::normal_distribution(0.0f, 0.5f);
-  state.wz.row(0) = Eigen::ArrayXf::NullaryExpr(30, [&] () {return normal_dist(engine);});
+  state.wz.row(0) = Eigen::ArrayXf::NullaryExpr(30, [&]() {return normal_dist(engine);});
   critic.score(data);
   EXPECT_NEAR(costs(0), 2.581, 4e-1);  // (mean of noise with mu=0, sigma=0.5 * 10.0 weight
 }
diff --git a/nav2_mppi_controller/test/models_test.cpp b/nav2_mppi_controller/test/models_test.cpp
index 90de9abb0fc..a556f2ba99f 100644
--- a/nav2_mppi_controller/test/models_test.cpp
+++ b/nav2_mppi_controller/test/models_test.cpp
@@ -140,7 +140,7 @@ TEST(ModelsTest, TrajectoriesTest)
   EXPECT_EQ(trajectories.yaws.cols(), 40);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp
index 82f8cb3dc50..7babbd9f5db 100644
--- a/nav2_mppi_controller/test/motion_model_tests.cpp
+++ b/nav2_mppi_controller/test/motion_model_tests.cpp
@@ -249,7 +249,7 @@ TEST(MotionModelTests, AckermannReversingTest)
   model.reset();
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp
index c5f7ffbcf41..b9daf8af5f3 100644
--- a/nav2_mppi_controller/test/noise_generator_test.cpp
+++ b/nav2_mppi_controller/test/noise_generator_test.cpp
@@ -201,7 +201,7 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMainNoRegenerate)
   generator.shutdown();
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp
index 9cfa986a85f..09551c387ae 100644
--- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp
+++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp
@@ -103,7 +103,7 @@ INSTANTIATE_TEST_SUITE_P(
       true))
 );
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp
index 901759449b8..598b53c8a0c 100644
--- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp
+++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp
@@ -177,13 +177,14 @@ class OptimizerTester : public Optimizer
     float max_delta_vx = settings_.constraints.ax_max * settings_.model_dt;
     float min_delta_vx = settings_.constraints.ax_min * settings_.model_dt;
     float max_delta_vy = settings_.constraints.ay_max * settings_.model_dt;
+    float min_delta_vy = settings_.constraints.ay_min * settings_.model_dt;
     float max_delta_wz = settings_.constraints.az_max * settings_.model_dt;
     propagateStateVelocitiesFromInitials(state);
     EXPECT_NEAR(state.vx(0, 0), 5.0, 1e-6);
     EXPECT_NEAR(state.vy(0, 0), 1.0, 1e-6);
     EXPECT_NEAR(state.wz(0, 0), 6.0, 1e-6);
     EXPECT_NEAR(state.vx(0, 1), std::clamp(0.75, 5.0 + min_delta_vx, 5.0 + max_delta_vx), 1e-6);
-    EXPECT_NEAR(state.vy(0, 1), std::clamp(0.5, 1.0 - max_delta_vy, 1.0 + max_delta_vy), 1e-6);
+    EXPECT_NEAR(state.vy(0, 1), std::clamp(0.5, 1.0 + min_delta_vy, 1.0 + max_delta_vy), 1e-6);
     EXPECT_NEAR(state.wz(0, 1), std::clamp(0.1, 6.0 - max_delta_wz, 6.0 + max_delta_wz), 1e-6);
 
     // Putting them together: updateStateVelocities
@@ -225,6 +226,7 @@ TEST(OptimizerTests, BasicInitializedFunctions)
   node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
   node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
   node->declare_parameter("mppic.ax_min", rclcpp::ParameterValue(3.0));
+  node->declare_parameter("mppic.ay_min", rclcpp::ParameterValue(3.0));
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
   ParametersHandler param_handler(node);
@@ -232,9 +234,10 @@ TEST(OptimizerTests, BasicInitializedFunctions)
   costmap_ros->on_configure(lstate);
   optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
 
-  // Test value of ax_min, it should be negative
+  // Test value of ax_min, ay_min it should be negative
   auto & constraints = optimizer_tester.getControlConstraints();
   EXPECT_EQ(constraints.ax_min, -3.0);
+  EXPECT_EQ(constraints.ay_min, -3.0);
 
   // Should be empty of size batches x time steps
   // and tests getting set params: time_steps, batch_size, controller_frequency
@@ -530,6 +533,7 @@ TEST(OptimizerTests, updateStateVelocitiesTests)
   node->declare_parameter("mppic.ax_max", rclcpp::ParameterValue(3.0));
   node->declare_parameter("mppic.ax_min", rclcpp::ParameterValue(-3.0));
   node->declare_parameter("mppic.ay_max", rclcpp::ParameterValue(3.0));
+  node->declare_parameter("mppic.ay_min", rclcpp::ParameterValue(-3.0));
   node->declare_parameter("mppic.az_max", rclcpp::ParameterValue(3.5));
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
@@ -629,7 +633,7 @@ TEST(OptimizerTests, integrateStateVelocitiesTests)
     EXPECT_NEAR(traj.y(1, i), i * 0.2 /*vel*/ * 0.1 /*dt*/, 1e-3);
   }
 
-  // Lets add some angular motion to the mix
+  // Let's add some angular motion to the mix
   state.vy = Eigen::ArrayXXf::Zero(1000, 50);
   state.wz = 0.2 * Eigen::ArrayXXf::Ones(1000, 50);
   state.wz.col(0) = Eigen::ArrayXf::Zero(1000);
@@ -646,7 +650,27 @@ TEST(OptimizerTests, integrateStateVelocitiesTests)
   }
 }
 
-int main(int argc, char **argv)
+TEST(OptimizerTests, TestGetters)
+{
+  OptimizerTester optimizer_tester;
+  optimizer_tester.fillOptimizerWithGarbage();
+
+  auto control_seq = optimizer_tester.getOptimalControlSequence();
+  EXPECT_EQ(control_seq.vx(0), 342.0);
+  EXPECT_EQ(control_seq.vx.rows(), 30);
+
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
+  node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  ParametersHandler param_handler(node);
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+  optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
+  EXPECT_EQ(optimizer_tester.getSettings().model_dt, 0.05f);
+}
+
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_mppi_controller/test/parameter_handler_test.cpp b/nav2_mppi_controller/test/parameter_handler_test.cpp
index 4fa3a479c92..56917bbb3c0 100644
--- a/nav2_mppi_controller/test/parameter_handler_test.cpp
+++ b/nav2_mppi_controller/test/parameter_handler_test.cpp
@@ -85,7 +85,7 @@ TEST(ParameterHandlerTest, PrePostDynamicCallbackTest)
     };
 
   auto dynamicCb = [&](const rclcpp::Parameter & /*param*/,
-    rcl_interfaces::msg::SetParametersResult & /*result*/) {
+      rcl_interfaces::msg::SetParametersResult & /*result*/) {
       dynamic_triggered = true;
     };
 
@@ -161,7 +161,8 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest)
     node->get_node_services_interface());
 
   std::shared_future<rcl_interfaces::msg::SetParametersResult> result_future =
-    rec_param->set_parameters_atomically({
+    rec_param->set_parameters_atomically(
+  {
     rclcpp::Parameter("my_node.verbose", true),
     rclcpp::Parameter("dynamic_int", 10),
     rclcpp::Parameter("static_int", 10)
@@ -175,7 +176,8 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest)
   auto result = result_future.get();
   EXPECT_EQ(result.successful, false);
   EXPECT_FALSE(result.reason.empty());
-  EXPECT_EQ(result.reason, std::string("Rejected change to static parameter: ") +
+  EXPECT_EQ(
+    result.reason, std::string("Rejected change to static parameter: ") +
     "{\"name\": \"static_int\", \"type\": \"integer\", \"value\": \"10\"}");
 
   // Now, only param1 should change, param 2 should be the same
@@ -206,7 +208,8 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersNotVerboseTest)
     node->get_node_services_interface());
 
   std::shared_future<rcl_interfaces::msg::SetParametersResult> result_future =
-    rec_param->set_parameters_atomically({
+    rec_param->set_parameters_atomically(
+  {
     // Don't set default param rclcpp::Parameter("my_node.verbose", false),
     rclcpp::Parameter("dynamic_int", 10),
     rclcpp::Parameter("static_int", 10)
@@ -220,7 +223,8 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersNotVerboseTest)
   auto result = result_future.get();
   EXPECT_EQ(result.successful, false);
   EXPECT_FALSE(result.reason.empty());
-  EXPECT_EQ(result.reason, std::string("Rejected change to static parameter: ") +
+  EXPECT_EQ(
+    result.reason, std::string("Rejected change to static parameter: ") +
     "{\"name\": \"static_int\", \"type\": \"integer\", \"value\": \"10\"}");
 
   // Now, only param1 should change, param 2 should be the same
@@ -245,7 +249,8 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersNotDeclaredTest)
     node->get_node_services_interface());
 
   std::shared_future<rcl_interfaces::msg::SetParametersResult>
-  result_future = rec_param->set_parameters_atomically({
+  result_future = rec_param->set_parameters_atomically(
+  {
     rclcpp::Parameter("my_node.verbose", true),
   });
 
@@ -259,7 +264,8 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersNotDeclaredTest)
   EXPECT_TRUE(result.reason.empty());
 
   // Try to set some parameters that have not been declared via the service client
-  result_future = rec_param->set_parameters_atomically({
+  result_future = rec_param->set_parameters_atomically(
+  {
     rclcpp::Parameter("static_int", 10),
     rclcpp::Parameter("not_declared", true),
     rclcpp::Parameter("not_declared2", true),
@@ -278,7 +284,7 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersNotDeclaredTest)
   EXPECT_EQ(result.reason, std::string("One or more parameters were not declared before setting"));
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp
index 01901510221..96bf9f7a627 100644
--- a/nav2_mppi_controller/test/path_handler_test.cpp
+++ b/nav2_mppi_controller/test/path_handler_test.cpp
@@ -240,7 +240,7 @@ TEST(PathHandlerTests, TestInversionToleranceChecks)
   EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose));
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp
index e9a30d9f237..c867ec678b9 100644
--- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp
+++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp
@@ -45,7 +45,7 @@ TEST(TrajectoryVisualizerTests, VisPathRepub)
   pub_path.poses.resize(5);
 
   auto my_sub = node->create_subscription<nav_msgs::msg::Path>(
-    "transformed_global_plan", 10,
+    "~/transformed_global_plan", 10,
     [&](const nav_msgs::msg::Path msg) {received_path = msg;});
 
   TrajectoryVisualizer vis;
@@ -65,7 +65,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
 
   visualization_msgs::msg::MarkerArray received_msg;
   auto my_sub = node->create_subscription<visualization_msgs::msg::MarkerArray>(
-    "trajectories", 10,
+    "~/candidate_trajectories", 10,
     [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;});
 
   // optimal_trajectory empty, should fail to publish
@@ -82,7 +82,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
   EXPECT_EQ(received_msg.markers.size(), 0u);
 
   // Now populated with content, should publish
-  optimal_trajectory = Eigen::ArrayXXf::Ones(20, 2);
+  optimal_trajectory = Eigen::ArrayXXf::Ones(20, 3);
   vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp);
   vis.visualize(bogus_path);
 
@@ -127,7 +127,7 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories)
 
   visualization_msgs::msg::MarkerArray received_msg;
   auto my_sub = node->create_subscription<visualization_msgs::msg::MarkerArray>(
-    "trajectories", 10,
+    "~/candidate_trajectories", 10,
     [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;});
 
   models::Trajectories candidate_trajectories;
@@ -157,7 +157,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath)
 
   nav_msgs::msg::Path received_path;
   auto my_sub = node->create_subscription<nav_msgs::msg::Path>(
-    "optimal_trajectory", 10,
+    "~/optimal_path", 10,
     [&](const nav_msgs::msg::Path msg) {received_path = msg;});
 
   // optimal_trajectory empty, should fail to publish
@@ -210,7 +210,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath)
   }
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp
index cc45443c7e0..c1114774652 100644
--- a/nav2_mppi_controller/test/utils_test.cpp
+++ b/nav2_mppi_controller/test/utils_test.cpp
@@ -339,7 +339,7 @@ TEST(UtilsTests, SmootherTest)
   std::mt19937 engine;
   std::normal_distribution<float> normal_dist = std::normal_distribution(0.0f, 0.2f);
   auto noises = Eigen::ArrayXf::NullaryExpr(
-    30, [&] () {return normal_dist(engine);});
+    30, [&]() {return normal_dist(engine);});
   noisey_sequence.vx += noises;
   noisey_sequence.vy += noises;
   noisey_sequence.wz += noises;
@@ -538,25 +538,29 @@ TEST(UtilsTests, NormalizeYawsBetweenPointsTest)
   yaw_between_points.setZero(10);
 
   // Try with both angles 0
-  Eigen::ArrayXf yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws,
+  Eigen::ArrayXf yaws_between_points_corrected = utils::normalize_yaws_between_points(
+    last_yaws,
     yaw_between_points);
   EXPECT_TRUE(yaws_between_points_corrected.isApprox(yaw_between_points));
 
   // Try with yaw between points as pi/4
   yaw_between_points.setConstant(M_PIF_2 / 2);
-  yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws,
+  yaws_between_points_corrected = utils::normalize_yaws_between_points(
+    last_yaws,
     yaw_between_points);
   EXPECT_TRUE(yaws_between_points_corrected.isApprox(yaw_between_points));
 
   // Try with yaw between points as pi/2
   yaw_between_points.setConstant(M_PIF_2);
-  yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws,
+  yaws_between_points_corrected = utils::normalize_yaws_between_points(
+    last_yaws,
     yaw_between_points);
   EXPECT_TRUE(yaws_between_points_corrected.isApprox(yaw_between_points));
 
   // Try with a few yaw between points  more than pi/2
   yaw_between_points[1] = 1.2 * M_PIF_2;
-  yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws,
+  yaws_between_points_corrected = utils::normalize_yaws_between_points(
+    last_yaws,
     yaw_between_points);
   EXPECT_NEAR(yaws_between_points_corrected[1], -0.8 * M_PIF_2, 1e-3);
   EXPECT_NEAR(yaws_between_points_corrected[0], yaw_between_points[0], 1e-3);
@@ -564,12 +568,72 @@ TEST(UtilsTests, NormalizeYawsBetweenPointsTest)
 
   // Try with goal angle 0
   float goal_angle = 0;
-  yaws_between_points_corrected = utils::normalize_yaws_between_points(goal_angle,
+  yaws_between_points_corrected = utils::normalize_yaws_between_points(
+    goal_angle,
     yaw_between_points);
   EXPECT_NEAR(yaws_between_points_corrected[1], -0.8 * M_PIF_2, 1e-3);
 }
 
-int main(int argc, char **argv)
+TEST(UtilsTests, toTrajectoryMsgTest)
+{
+  Eigen::ArrayXXf trajectory(5, 3);
+  trajectory <<
+    0.0, 0.0, 0.0,
+    1.0, 1.0, 1.0,
+    2.0, 2.0, 2.0,
+    3.0, 3.0, 3.0,
+    4.0, 4.0, 4.0;
+
+  models::ControlSequence control_sequence;
+  control_sequence.vx = Eigen::ArrayXf::Ones(5);
+  control_sequence.wz = Eigen::ArrayXf::Ones(5);
+  control_sequence.vy = Eigen::ArrayXf::Zero(5);
+
+  std_msgs::msg::Header header;
+  header.frame_id = "map";
+  header.stamp = rclcpp::Time(100, 0, RCL_ROS_TIME);
+
+  auto trajectory_msg = utils::toTrajectoryMsg(
+    trajectory, control_sequence, 1.0, header);
+
+  EXPECT_EQ(trajectory_msg->header.frame_id, "map");
+  EXPECT_EQ(trajectory_msg->header.stamp, header.stamp);
+  EXPECT_EQ(trajectory_msg->points.size(), 5u);
+  EXPECT_EQ(trajectory_msg->points[0].pose.position.x, 0.0);
+  EXPECT_EQ(trajectory_msg->points[0].pose.position.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[1].pose.position.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[1].pose.position.y, 1.0);
+  EXPECT_EQ(trajectory_msg->points[2].pose.position.x, 2.0);
+  EXPECT_EQ(trajectory_msg->points[2].pose.position.y, 2.0);
+  EXPECT_EQ(trajectory_msg->points[3].pose.position.x, 3.0);
+  EXPECT_EQ(trajectory_msg->points[3].pose.position.y, 3.0);
+  EXPECT_EQ(trajectory_msg->points[4].pose.position.x, 4.0);
+  EXPECT_EQ(trajectory_msg->points[4].pose.position.y, 4.0);
+
+  EXPECT_EQ(trajectory_msg->points[0].velocity.linear.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[0].velocity.linear.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[0].velocity.angular.z, 1.0);
+  EXPECT_EQ(trajectory_msg->points[1].velocity.linear.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[1].velocity.linear.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[1].velocity.angular.z, 1.0);
+  EXPECT_EQ(trajectory_msg->points[2].velocity.linear.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[2].velocity.linear.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[2].velocity.angular.z, 1.0);
+  EXPECT_EQ(trajectory_msg->points[3].velocity.linear.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[3].velocity.linear.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[3].velocity.angular.z, 1.0);
+  EXPECT_EQ(trajectory_msg->points[4].velocity.linear.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[4].velocity.linear.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[4].velocity.angular.z, 1.0);
+
+  EXPECT_EQ(trajectory_msg->points[0].time_from_start, rclcpp::Duration(0, 0));
+  EXPECT_EQ(trajectory_msg->points[1].time_from_start, rclcpp::Duration(1, 0));
+  EXPECT_EQ(trajectory_msg->points[2].time_from_start, rclcpp::Duration(2, 0));
+  EXPECT_EQ(trajectory_msg->points[3].time_from_start, rclcpp::Duration(3, 0));
+  EXPECT_EQ(trajectory_msg->points[4].time_from_start, rclcpp::Duration(4, 0));
+}
+
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt
index 1e546801f52..4319129ecbc 100644
--- a/nav2_msgs/CMakeLists.txt
+++ b/nav2_msgs/CMakeLists.txt
@@ -31,7 +31,9 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/BehaviorTreeLog.msg"
   "msg/Particle.msg"
   "msg/ParticleCloud.msg"
-  "msg/MissedWaypoint.msg"
+  "msg/Trajectory.msg"
+  "msg/TrajectoryPoint.msg"
+  "msg/WaypointStatus.msg"
   "srv/GetCosts.srv"
   "srv/GetCostmap.srv"
   "srv/IsPathValid.srv"
diff --git a/nav2_msgs/action/FollowGPSWaypoints.action b/nav2_msgs/action/FollowGPSWaypoints.action
index c53d4faa854..7512b247be9 100644
--- a/nav2_msgs/action/FollowGPSWaypoints.action
+++ b/nav2_msgs/action/FollowGPSWaypoints.action
@@ -13,7 +13,7 @@ uint16 TASK_EXECUTOR_FAILED=601
 uint16 NO_WAYPOINTS_GIVEN=602
 uint16 STOP_ON_MISSED_WAYPOINT=603
 
-MissedWaypoint[] missed_waypoints
+WaypointStatus[] missed_waypoints
 int16 error_code
 string error_msg
 ---
diff --git a/nav2_msgs/action/FollowWaypoints.action b/nav2_msgs/action/FollowWaypoints.action
index a45e13401f4..cbb2a381d5b 100644
--- a/nav2_msgs/action/FollowWaypoints.action
+++ b/nav2_msgs/action/FollowWaypoints.action
@@ -13,7 +13,7 @@ uint16 TASK_EXECUTOR_FAILED=601
 uint16 NO_VALID_WAYPOINTS=602
 uint16 STOP_ON_MISSED_WAYPOINT=603
 
-MissedWaypoint[] missed_waypoints
+WaypointStatus[] missed_waypoints
 uint16 error_code
 string error_msg
 ---
diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action
index a4301f36e4a..8bbaa361e73 100644
--- a/nav2_msgs/action/NavigateThroughPoses.action
+++ b/nav2_msgs/action/NavigateThroughPoses.action
@@ -14,6 +14,7 @@ uint16 TF_ERROR=9102
 
 uint16 error_code
 string error_msg
+WaypointStatus[] waypoint_statuses
 ---
 #feedback definition
 geometry_msgs/PoseStamped current_pose
@@ -22,3 +23,4 @@ builtin_interfaces/Duration estimated_time_remaining
 int16 number_of_recoveries
 float32 distance_remaining
 int16 number_of_poses_remaining
+WaypointStatus[] waypoint_statuses
diff --git a/nav2_msgs/action/__init__.py b/nav2_msgs/action/__init__.py
new file mode 100644
index 00000000000..a4036af8cda
--- /dev/null
+++ b/nav2_msgs/action/__init__.py
@@ -0,0 +1,35 @@
+from nav2_msgs.action._assisted_teleop import AssistedTeleop
+from nav2_msgs.action._back_up import BackUp
+from nav2_msgs.action._compute_path_through_poses import ComputePathThroughPoses
+from nav2_msgs.action._compute_path_to_pose import ComputePathToPose
+from nav2_msgs.action._dock_robot import DockRobot
+from nav2_msgs.action._drive_on_heading import DriveOnHeading
+from nav2_msgs.action._dummy_behavior import DummyBehavior
+from nav2_msgs.action._follow_gps_waypoints import FollowGPSWaypoints
+from nav2_msgs.action._follow_path import FollowPath
+from nav2_msgs.action._follow_waypoints import FollowWaypoints
+from nav2_msgs.action._navigate_through_poses import NavigateThroughPoses
+from nav2_msgs.action._navigate_to_pose import NavigateToPose
+from nav2_msgs.action._smooth_path import SmoothPath
+from nav2_msgs.action._spin import Spin
+from nav2_msgs.action._undock_robot import UndockRobot
+from nav2_msgs.action._wait import Wait
+
+__all__ = [
+    'AssistedTeleop',
+    'BackUp',
+    'ComputePathThroughPoses',
+    'ComputePathToPose',
+    'DockRobot',
+    'DriveOnHeading',
+    'DummyBehavior',
+    'FollowGPSWaypoints',
+    'FollowPath',
+    'FollowWaypoints',
+    'NavigateThroughPoses',
+    'NavigateToPose',
+    'SmoothPath',
+    'Spin',
+    'UndockRobot',
+    'Wait',
+]
diff --git a/nav2_msgs/msg/MissedWaypoint.msg b/nav2_msgs/msg/MissedWaypoint.msg
deleted file mode 100644
index 46846a22125..00000000000
--- a/nav2_msgs/msg/MissedWaypoint.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-uint32 index
-geometry_msgs/PoseStamped goal
-uint16 error_code
-string error_msg
diff --git a/nav2_msgs/msg/Trajectory.msg b/nav2_msgs/msg/Trajectory.msg
new file mode 100644
index 00000000000..808173d71e2
--- /dev/null
+++ b/nav2_msgs/msg/Trajectory.msg
@@ -0,0 +1,7 @@
+# An array of trajectory points that represents a trajectory for a robot to follow.
+
+# Indicates the frame_id of the trajectory.
+std_msgs/Header header
+
+# Array of trajectory points to follow.
+TrajectoryPoint[] points
diff --git a/nav2_msgs/msg/TrajectoryPoint.msg b/nav2_msgs/msg/TrajectoryPoint.msg
new file mode 100644
index 00000000000..afe672a6a5c
--- /dev/null
+++ b/nav2_msgs/msg/TrajectoryPoint.msg
@@ -0,0 +1,16 @@
+# Trajectory point state
+
+# Desired time from the trajectory start to arrive at this trajectory sample.
+builtin_interfaces/Duration time_from_start
+
+# Pose of the trajectory sample.
+geometry_msgs/Pose pose
+
+# Velocity of the trajectory sample.
+geometry_msgs/Twist velocity
+
+# Acceleration of the trajectory (optional).
+geometry_msgs/Accel acceleration
+
+# Force/Torque to apply at trajectory sample (optional).
+geometry_msgs/Wrench effort
diff --git a/nav2_msgs/msg/WaypointStatus.msg b/nav2_msgs/msg/WaypointStatus.msg
new file mode 100644
index 00000000000..a8863b69669
--- /dev/null
+++ b/nav2_msgs/msg/WaypointStatus.msg
@@ -0,0 +1,10 @@
+# Waypoint state for multi-goal navigation or waypoint following
+uint8 PENDING=0 # Waypoint is not processed or processing
+uint8 COMPLETED=1 # Waypoint is completed
+uint8 SKIPPED=2 # Waypoint is skipped
+uint8 FAILED=3 # Waypoint is failed
+uint8 waypoint_status
+uint32 waypoint_index
+geometry_msgs/PoseStamped waypoint_pose
+uint16 error_code
+string error_msg
diff --git a/nav2_msgs/msg/__init__.py b/nav2_msgs/msg/__init__.py
new file mode 100644
index 00000000000..a6e8fcd4219
--- /dev/null
+++ b/nav2_msgs/msg/__init__.py
@@ -0,0 +1,29 @@
+from nav2_msgs.msg._behavior_tree_log import BehaviorTreeLog
+from nav2_msgs.msg._behavior_tree_status_change import BehaviorTreeStatusChange
+from nav2_msgs.msg._collision_detector_state import CollisionDetectorState
+from nav2_msgs.msg._collision_monitor_state import CollisionMonitorState
+from nav2_msgs.msg._costmap import Costmap
+from nav2_msgs.msg._costmap_filter_info import CostmapFilterInfo
+from nav2_msgs.msg._costmap_meta_data import CostmapMetaData
+from nav2_msgs.msg._costmap_update import CostmapUpdate
+from nav2_msgs.msg._missed_waypoint import MissedWaypoint
+from nav2_msgs.msg._particle import Particle
+from nav2_msgs.msg._particle_cloud import ParticleCloud
+from nav2_msgs.msg._speed_limit import SpeedLimit
+from nav2_msgs.msg._voxel_grid import VoxelGrid
+
+__all__ = [
+    'BehaviorTreeLog',
+    'BehaviorTreeStatusChange',
+    'CollisionDetectorState',
+    'CollisionMonitorState',
+    'Costmap',
+    'CostmapFilterInfo',
+    'CostmapMetaData',
+    'CostmapUpdate',
+    'MissedWaypoint',
+    'Particle',
+    'ParticleCloud',
+    'SpeedLimit',
+    'VoxelGrid',
+]
diff --git a/nav2_msgs/srv/GetCosts.srv b/nav2_msgs/srv/GetCosts.srv
index c5ca48a3099..93f0f360161 100644
--- a/nav2_msgs/srv/GetCosts.srv
+++ b/nav2_msgs/srv/GetCosts.srv
@@ -4,4 +4,4 @@ bool use_footprint
 geometry_msgs/PoseStamped[] poses
 ---
 float32[] costs
-bool success
\ No newline at end of file
+bool success
diff --git a/nav2_msgs/srv/IsPathValid.srv b/nav2_msgs/srv/IsPathValid.srv
index e178847085a..a51bf949114 100644
--- a/nav2_msgs/srv/IsPathValid.srv
+++ b/nav2_msgs/srv/IsPathValid.srv
@@ -5,4 +5,4 @@ uint8 max_cost 253
 bool consider_unknown_as_obstacle false
 ---
 bool is_valid
-int32[] invalid_pose_indices 
+int32[] invalid_pose_indices
diff --git a/nav2_msgs/srv/SetInitialPose.srv b/nav2_msgs/srv/SetInitialPose.srv
index ec1e50b65f1..7089d1a5871 100644
--- a/nav2_msgs/srv/SetInitialPose.srv
+++ b/nav2_msgs/srv/SetInitialPose.srv
@@ -1,3 +1,2 @@
 geometry_msgs/PoseWithCovarianceStamped pose
 ---
-
diff --git a/nav2_msgs/srv/__init__.py b/nav2_msgs/srv/__init__.py
new file mode 100644
index 00000000000..51658c3f3b8
--- /dev/null
+++ b/nav2_msgs/srv/__init__.py
@@ -0,0 +1,25 @@
+from nav2_msgs.srv._clear_costmap_around_robot import ClearCostmapAroundRobot
+from nav2_msgs.srv._clear_costmap_except_region import ClearCostmapExceptRegion
+from nav2_msgs.srv._clear_entire_costmap import ClearEntireCostmap
+from nav2_msgs.srv._get_costmap import GetCostmap
+from nav2_msgs.srv._get_costs import GetCosts
+from nav2_msgs.srv._is_path_valid import IsPathValid
+from nav2_msgs.srv._load_map import LoadMap
+from nav2_msgs.srv._manage_lifecycle_nodes import ManageLifecycleNodes
+from nav2_msgs.srv._reload_dock_database import ReloadDockDatabase
+from nav2_msgs.srv._save_map import SaveMap
+from nav2_msgs.srv._set_initial_pose import SetInitialPose
+
+__all__ = [
+    'ClearCostmapAroundRobot',
+    'ClearCostmapExceptRegion',
+    'ClearEntireCostmap',
+    'GetCostmap',
+    'GetCosts',
+    'IsPathValid',
+    'LoadMap',
+    'ManageLifecycleNodes',
+    'ReloadDockDatabase',
+    'SaveMap',
+    'SetInitialPose',
+]
diff --git a/nav2_navfn_planner/README.md b/nav2_navfn_planner/README.md
index 8b4a07e051d..68dd5240e77 100644
--- a/nav2_navfn_planner/README.md
+++ b/nav2_navfn_planner/README.md
@@ -2,6 +2,6 @@
 
 The NavfnPlanner is a global planner plugin for the Nav2 Planner server. It implements the Navigation Function planner with either A\* or Dij. expansions. It is largely equivalent to its counterpart in ROS 1 Navigation. The Navfn planner assumes a circular robot (or a robot that can be approximated as circular for the purposes of global path planning) and operates on a weighted costmap.
 
-The `global_planner` package from ROS (1) is a refactor on NavFn to make it more easily understandable, but it lacks in run-time performance and introduces suboptimal behaviors. As NavFn has been extremely stable for about 10 years at the time of porting, the maintainers felt no compelling reason to port over another, largely equivalent (but poorer functioning) planner. 
+The `global_planner` package from ROS (1) is a refactor on NavFn to make it more easily understandable, but it lacks in run-time performance and introduces suboptimal behaviors. As NavFn has been extremely stable for about 10 years at the time of porting, the maintainers felt no compelling reason to port over another, largely equivalent (but poorer functioning) planner.
 
 See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-navfn.html) for additional parameter descriptions.
diff --git a/nav2_navfn_planner/src/navfn.cpp b/nav2_navfn_planner/src/navfn.cpp
index d166dbc7b1b..2cce713b0f2 100644
--- a/nav2_navfn_planner/src/navfn.cpp
+++ b/nav2_navfn_planner/src/navfn.cpp
@@ -326,14 +326,14 @@ int NavFn::getPathLen() {return npath;}
 
 // inserting onto the priority blocks
 #define push_cur(n)  {if (n >= 0 && n < ns && !pending[n] && \
-  costarr[n] < COST_OBS && curPe < PRIORITYBUFSIZE) \
-  {curP[curPe++] = n; pending[n] = true;}}
+      costarr[n] < COST_OBS && curPe < PRIORITYBUFSIZE) \
+    {curP[curPe++] = n; pending[n] = true;}}
 #define push_next(n) {if (n >= 0 && n < ns && !pending[n] && \
-  costarr[n] < COST_OBS && nextPe < PRIORITYBUFSIZE) \
-  {nextP[nextPe++] = n; pending[n] = true;}}
+      costarr[n] < COST_OBS && nextPe < PRIORITYBUFSIZE) \
+    {nextP[nextPe++] = n; pending[n] = true;}}
 #define push_over(n) {if (n >= 0 && n < ns && !pending[n] && \
-  costarr[n] < COST_OBS && overPe < PRIORITYBUFSIZE) \
-  {overP[overPe++] = n; pending[n] = true;}}
+      costarr[n] < COST_OBS && overPe < PRIORITYBUFSIZE) \
+    {overP[overPe++] = n; pending[n] = true;}}
 
 
 // Set up navigation potential arrays for new propagation
diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp
index 7c94751a761..e01261434d1 100644
--- a/nav2_navfn_planner/src/navfn_planner.cpp
+++ b/nav2_navfn_planner/src/navfn_planner.cpp
@@ -293,11 +293,13 @@ NavfnPlanner::makePlan(
       p.position.x = goal.position.x - tolerance;
       while (p.position.x <= goal.position.x + tolerance) {
         potential = getPointPotential(p.position);
-        double sdist = squared_distance(p, goal);
-        if (potential < POT_HIGH && sdist < best_sdist) {
-          best_sdist = sdist;
-          best_pose = p;
-          found_legal = true;
+        if (potential < POT_HIGH) {
+          double sdist = squared_distance(p, goal);
+          if (sdist < best_sdist) {
+            best_sdist = sdist;
+            best_pose = p;
+            found_legal = true;
+          }
         }
         p.position.x += resolution;
       }
@@ -366,6 +368,7 @@ NavfnPlanner::smoothApproachToGoal(
   }
   geometry_msgs::msg::PoseStamped goal_copy;
   goal_copy.pose = goal;
+  goal_copy.header = plan.header;
   plan.poses.push_back(goal_copy);
 }
 
@@ -415,6 +418,7 @@ NavfnPlanner::getPlanFromPotential(
     mapToWorld(x[i], y[i], world_x, world_y);
 
     geometry_msgs::msg::PoseStamped pose;
+    pose.header = plan.header;
     pose.pose.position.x = world_x;
     pose.pose.position.y = world_y;
     pose.pose.position.z = 0.0;
diff --git a/nav2_navfn_planner/test/test_dynamic_parameters.cpp b/nav2_navfn_planner/test/test_dynamic_parameters.cpp
index 543167543ed..2c15011a2af 100644
--- a/nav2_navfn_planner/test/test_dynamic_parameters.cpp
+++ b/nav2_navfn_planner/test/test_dynamic_parameters.cpp
@@ -55,7 +55,7 @@ TEST(NavfnTest, testDynamicParameter)
   EXPECT_EQ(node->get_parameter("test.use_final_approach_orientation").as_bool(), true);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_planner/README.md b/nav2_planner/README.md
index 302fed50db1..50efaf3bb7e 100644
--- a/nav2_planner/README.md
+++ b/nav2_planner/README.md
@@ -4,6 +4,6 @@ The Nav2 planner is a Task Server in Nav2 that implements the `nav2_behavior_tre
 
 A planning module implementing the `nav2_behavior_tree::ComputePathToPose` interface is responsible for generating a feasible path given start and end robot poses. It loads a map of potential planner plugins to do the path generation in different user-defined situations.
 
-See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. 
+See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins.
 
 See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-planner-server.html) for additional parameter descriptions and a [tutorial about writing planner plugins](https://docs.nav2.org/plugin_tutorials/docs/writing_new_nav2planner_plugin.html).
diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp
index 8d5ce2903e9..21bc032e5fa 100644
--- a/nav2_planner/include/nav2_planner/planner_server.hpp
+++ b/nav2_planner/include/nav2_planner/planner_server.hpp
@@ -31,6 +31,7 @@
 #include "nav2_msgs/msg/costmap.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_util/simple_action_server.hpp"
+#include "nav2_util/service_server.hpp"
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/create_timer_ros.h"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
@@ -204,6 +205,7 @@ class PlannerServer : public nav2_util::LifecycleNode
    * @param response from the service
    */
   void isPathValid(
+    const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<nav2_msgs::srv::IsPathValid::Request> request,
     std::shared_ptr<nav2_msgs::srv::IsPathValid::Response> response);
 
@@ -260,7 +262,8 @@ class PlannerServer : public nav2_util::LifecycleNode
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;
 
   // Service to determine if the path is valid
-  rclcpp::Service<nav2_msgs::srv::IsPathValid>::SharedPtr is_path_valid_service_;
+  nav2_util::ServiceServer<nav2_msgs::srv::IsPathValid,
+    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr is_path_valid_service_;
 };
 
 }  // namespace nav2_planner
diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
index be65ffef9c8..f9b0689378e 100644
--- a/nav2_planner/src/planner_server.cpp
+++ b/nav2_planner/src/planner_server.cpp
@@ -197,11 +197,13 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
 
   auto node = shared_from_this();
 
-  is_path_valid_service_ = node->create_service<nav2_msgs::srv::IsPathValid>(
+  is_path_valid_service_ = std::make_shared<nav2_util::ServiceServer<nav2_msgs::srv::IsPathValid,
+      std::shared_ptr<nav2_util::LifecycleNode>>>(
     "is_path_valid",
+    node,
     std::bind(
-      &PlannerServer::isPathValid, this,
-      std::placeholders::_1, std::placeholders::_2));
+      &PlannerServer::isPathValid, this, std::placeholders::_1, std::placeholders::_2,
+      std::placeholders::_3));
 
   // Add callback for dynamic parameters
   dyn_params_handler_ = node->add_on_set_parameters_callback(
@@ -249,6 +251,7 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Cleaning up");
 
+  is_path_valid_service_.reset();
   action_server_pose_.reset();
   action_server_poses_.reset();
   plan_publisher_.reset();
@@ -645,6 +648,7 @@ PlannerServer::publishPlan(const nav_msgs::msg::Path & path)
 }
 
 void PlannerServer::isPathValid(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
   const std::shared_ptr<nav2_msgs::srv::IsPathValid::Request> request,
   std::shared_ptr<nav2_msgs::srv::IsPathValid::Response> response)
 {
diff --git a/nav2_planner/test/test_dynamic_parameters.cpp b/nav2_planner/test/test_dynamic_parameters.cpp
index 61f327e8094..050cc4ef92a 100644
--- a/nav2_planner/test/test_dynamic_parameters.cpp
+++ b/nav2_planner/test/test_dynamic_parameters.cpp
@@ -81,7 +81,7 @@ TEST(WPTest, test_dynamic_parameters)
   EXPECT_EQ(planner->get_parameter("expected_planner_frequency").as_double(), -1.0);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md
index 81244d804df..f49f9b31420 100644
--- a/nav2_regulated_pure_pursuit_controller/README.md
+++ b/nav2_regulated_pure_pursuit_controller/README.md
@@ -22,7 +22,7 @@ If you use the Regulated Pure Pursuit Controller algorithm or software from this
 
 ```bibtex
 @article{macenski2023regulated,
-      title={Regulated Pure Pursuit for Robot Path Tracking}, 
+      title={Regulated Pure Pursuit for Robot Path Tracking},
       author={Steve Macenski and Shrijit Singh and Francisco Martin and Jonatan Gines},
       year={2023},
       journal = {Autonomous Robots}
@@ -31,7 +31,7 @@ If you use the Regulated Pure Pursuit Controller algorithm or software from this
 
 ## Pure Pursuit Basics
 
-The Pure Pursuit algorithm has been in use for over 30 years. You can read more about the details of the pure pursuit controller in its [introduction paper](http://www.enseignement.polytechnique.fr/profs/informatique/Eric.Goubault/MRIS/coulter_r_craig_1992_1.pdf). The core idea is to find a point on the path in front of the robot and find the linear and angular velocity to help drive towards it. Once it moves forward, a new point is selected, and the process repeats until the end of the path. The distance used to find the point to drive towards is the `lookahead` distance. 
+The Pure Pursuit algorithm has been in use for over 30 years. You can read more about the details of the pure pursuit controller in its [introduction paper](http://www.enseignement.polytechnique.fr/profs/informatique/Eric.Goubault/MRIS/coulter_r_craig_1992_1.pdf). The core idea is to find a point on the path in front of the robot and find the linear and angular velocity to help drive towards it. Once it moves forward, a new point is selected, and the process repeats until the end of the path. The distance used to find the point to drive towards is the `lookahead` distance.
 
 In order to simply book-keeping, the global path is continuously pruned to the closest point to the robot (see the figure below) so that we only have to process useful path points. Then, the section of the path within the local costmap bounds is transformed to the robot frame and a lookahead point is determined using a predefined distance.
 
@@ -55,42 +55,42 @@ The major improvements that this work implements is the regulations on the linea
 
 The cost functions penalize the robot's speed based on its proximity to obstacles and the curvature of the path. This is helpful to slow the robot when moving close to things in narrow spaces and scaling down the linear velocity by curvature helps to stabilize the controller over a larger range of lookahead point distances. This also has the added benefit of removing the sensitive tuning of the lookahead point / range, as the robot will track paths far better. Tuning is still required, but it is substantially easier to get reasonable behavior with minor adjustments.
 
-An unintended tertiary benefit of scaling the linear velocities by curvature is that a robot will natively rotate to rough path heading when using holonomic planners that don't start aligned with the robot pose orientation. As the curvature will be very high, the linear velocity drops and the angular velocity takes over to rotate to heading. While not perfect, it does dramatically reduce the need to rotate to a close path heading before following and opens up a broader range of planning techniques. Pure Pursuit controllers otherwise would be completely unable to recover from this in even modestly confined spaces. 
+An unintended tertiary benefit of scaling the linear velocities by curvature is that a robot will natively rotate to rough path heading when using holonomic planners that don't start aligned with the robot pose orientation. As the curvature will be very high, the linear velocity drops and the angular velocity takes over to rotate to heading. While not perfect, it does dramatically reduce the need to rotate to a close path heading before following and opens up a broader range of planning techniques. Pure Pursuit controllers otherwise would be completely unable to recover from this in even modestly confined spaces.
 
-Mixing the proximity and curvature regulated linear velocities with the time-scaled collision checker, we see a near-perfect combination allowing the regulated pure pursuit algorithm to handle high starting deviations from the path and navigate collision-free in tight spaces without overshoot. 
+Mixing the proximity and curvature regulated linear velocities with the time-scaled collision checker, we see a near-perfect combination allowing the regulated pure pursuit algorithm to handle high starting deviations from the path and navigate collision-free in tight spaces without overshoot.
 
 Note: The maximum allowed time to collision is thresholded by the lookahead point, starting in Humble. This is such that collision checking isn't significantly overshooting the path, which can cause issues in constrained environments. For example, if there were a straight-line path going towards a wall that then turned left, if this parameter was set to high, then it would detect a collision past the point of actual robot intended motion. Thusly, if a robot is moving fast, selecting further out lookahead points is not only a matter of behavioral stability for Pure Pursuit, but also gives a robot further predictive collision detection capabilities. The max allowable time parameter is still in place for slow commands, as described in detail above.
 
 ## Configuration
 
-| Parameter | Description | 
+| Parameter | Description |
 |-----|----|
-| `desired_linear_vel` | The desired maximum linear velocity to use. | 
-| `lookahead_dist` | The lookahead distance to use to find the lookahead point | 
-| `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances | 
-| `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances | 
-| `lookahead_time` | The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain. | 
-| `rotate_to_heading_angular_vel` | If rotate to heading is used, this is the angular velocity to use. | 
-| `transform_tolerance` | The TF transform tolerance | 
-| `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` | 
-| `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal | 
-| `approach_velocity_scaling_dist` | Integrated distance from end of transformed path at which to start applying velocity scaling. This defaults to the forward extent of the costmap minus one costmap cell length. | 
+| `desired_linear_vel` | The desired maximum linear velocity to use. |
+| `lookahead_dist` | The lookahead distance to use to find the lookahead point |
+| `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances |
+| `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances |
+| `lookahead_time` | The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain. |
+| `rotate_to_heading_angular_vel` | If rotate to heading is used, this is the angular velocity to use. |
+| `transform_tolerance` | The TF transform tolerance |
+| `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` |
+| `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal |
+| `approach_velocity_scaling_dist` | Integrated distance from end of transformed path at which to start applying velocity scaling. This defaults to the forward extent of the costmap minus one costmap cell length. |
 | `use_collision_detection` | Whether to enable collision detection. |
 | `max_allowed_time_to_collision_up_to_carrot` | The time to project a velocity command to check for collisions when `use_collision_detection` is `true`. It is limited to maximum distance of lookahead distance selected. |
-| `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature | 
-| `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles | 
-| `cost_scaling_dist` | The minimum distance from an obstacle to trigger the scaling of linear velocity, if `use_cost_regulated_linear_velocity_scaling` is enabled. The value set should be smaller or equal to the `inflation_radius` set in the inflation layer of costmap, since inflation is used to compute the distance from obstacles | 
-| `cost_scaling_gain` | A multiplier gain, which should be <= 1.0, used to further scale the speed when an obstacle is within `cost_scaling_dist`. Lower value reduces speed more quickly. | 
-| `inflation_cost_scaling_factor` | The value of `cost_scaling_factor` set for the inflation layer in the local costmap. The value should be exactly the same for accurately computing distance from obstacles using the inflated cell values | 
-| `regulated_linear_scaling_min_radius` | The turning radius for which the regulation features are triggered. Remember, sharper turns have smaller radii | 
-| `regulated_linear_scaling_min_speed` | The minimum speed for which the regulated features can send, to ensure process is still achievable even in high cost spaces with high curvature. | 
-| `use_fixed_curvature_lookahead` | Enable fixed lookahead for curvature detection. Useful for systems with long lookahead. | 
-| `curvature_lookahead_dist` | Distance to lookahead to determine curvature for velocity regulation purposes. Only used if `use_fixed_curvature_lookahead` is enabled. | 
-| `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. | 
-| `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | 
-| `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | 
-| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | 
-| `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvature calculation (to avoid oscillations at the end of the path) | 
+| `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature |
+| `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles |
+| `cost_scaling_dist` | The minimum distance from an obstacle to trigger the scaling of linear velocity, if `use_cost_regulated_linear_velocity_scaling` is enabled. The value set should be smaller or equal to the `inflation_radius` set in the inflation layer of costmap, since inflation is used to compute the distance from obstacles |
+| `cost_scaling_gain` | A multiplier gain, which should be <= 1.0, used to further scale the speed when an obstacle is within `cost_scaling_dist`. Lower value reduces speed more quickly. |
+| `inflation_cost_scaling_factor` | The value of `cost_scaling_factor` set for the inflation layer in the local costmap. The value should be exactly the same for accurately computing distance from obstacles using the inflated cell values |
+| `regulated_linear_scaling_min_radius` | The turning radius for which the regulation features are triggered. Remember, sharper turns have smaller radii |
+| `regulated_linear_scaling_min_speed` | The minimum speed for which the regulated features can send, to ensure process is still achievable even in high cost spaces with high curvature. |
+| `use_fixed_curvature_lookahead` | Enable fixed lookahead for curvature detection. Useful for systems with long lookahead. |
+| `curvature_lookahead_dist` | Distance to lookahead to determine curvature for velocity regulation purposes. Only used if `use_fixed_curvature_lookahead` is enabled. |
+| `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. |
+| `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. |
+| `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled |
+| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. |
+| `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvature calculation (to avoid oscillations at the end of the path) |
 
 Example fully-described XML with default parameter values:
 
@@ -146,10 +146,10 @@ controller_server:
 
 ## Topics
 
-| Topic  | Type | Description | 
+| Topic  | Type | Description |
 |-----|----|----|
-| `lookahead_point`  | `geometry_msgs/PointStamped` | The current lookahead point on the path | 
-| `lookahead_arc`  | `nav_msgs/Path` | The drivable arc between the robot and the carrot. Arc length depends on `max_allowed_time_to_collision_up_to_carrot`, forward simulating from the robot pose at the commanded `Twist` by that time. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. | 
+| `lookahead_point`  | `geometry_msgs/PointStamped` | The current lookahead point on the path |
+| `lookahead_arc`  | `nav_msgs/Path` | The drivable arc between the robot and the carrot. Arc length depends on `max_allowed_time_to_collision_up_to_carrot`, forward simulating from the robot pose at the commanded `Twist` by that time. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. |
 
 Note: The `lookahead_arc` is also a really great speed indicator, when "full" to carrot or max time, you know you're at full speed. If 20% less, you can tell the robot is approximately 20% below maximum speed. Think of it as the collision checking bounds but also a speed gauge.
 
diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp
index 66a53123436..7b1be9855d2 100644
--- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp
+++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp
@@ -93,12 +93,14 @@ class ParameterHandler
    * @brief Callback executed when a parameter change is detected
    * @param event ParameterEvent message
    */
+  void
+  updateParametersCallback(std::vector<rclcpp::Parameter> parameters);
   rcl_interfaces::msg::SetParametersResult
-  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
-
+  validateParameterUpdatesCallback(std::vector<rclcpp::Parameter> parameters);
   // Dynamic parameters handler
   std::mutex mutex_;
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+  rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_;
   Parameters params_;
   std::string plugin_name_;
   rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")};
diff --git a/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml b/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml
index d695bcec7f8..b7371017e6e 100644
--- a/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml
+++ b/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml
@@ -7,4 +7,3 @@
         </class>
     </library>
 </class_libraries>
-
diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp
index 298ff2bf93a..35201b99ba4 100644
--- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp
+++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp
@@ -189,40 +189,75 @@ ParameterHandler::ParameterHandler(
     params_.use_cost_regulated_linear_velocity_scaling = false;
   }
 
-  dyn_params_handler_ = node->add_on_set_parameters_callback(
+  post_set_params_handler_ = node->add_post_set_parameters_callback(
     std::bind(
-      &ParameterHandler::dynamicParametersCallback,
+      &ParameterHandler::updateParametersCallback,
+      this, std::placeholders::_1));
+  on_set_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &ParameterHandler::validateParameterUpdatesCallback,
       this, std::placeholders::_1));
 }
 
 ParameterHandler::~ParameterHandler()
 {
   auto node = node_.lock();
-  if (dyn_params_handler_ && node) {
-    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  if (post_set_params_handler_ && node) {
+    node->remove_post_set_parameters_callback(post_set_params_handler_.get());
+  }
+  post_set_params_handler_.reset();
+  if (on_set_params_handler_ && node) {
+    node->remove_on_set_parameters_callback(on_set_params_handler_.get());
   }
-  dyn_params_handler_.reset();
+  on_set_params_handler_.reset();
 }
-
-rcl_interfaces::msg::SetParametersResult
-ParameterHandler::dynamicParametersCallback(
+rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback(
   std::vector<rclcpp::Parameter> parameters)
 {
   rcl_interfaces::msg::SetParametersResult result;
-  std::lock_guard<std::mutex> lock_reinit(mutex_);
-
+  result.successful = true;
   for (auto parameter : parameters) {
     const auto & type = parameter.get_type();
     const auto & name = parameter.get_name();
 
     if (type == ParameterType::PARAMETER_DOUBLE) {
-      if (name == plugin_name_ + ".inflation_cost_scaling_factor") {
-        if (parameter.as_double() <= 0.0) {
+      if (name == plugin_name_ + ".inflation_cost_scaling_factor" && parameter.as_double() <= 0.0) {
+        RCLCPP_WARN(
+          logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
+          "it should be >0. Ignoring parameter update.");
+        result.successful = false;
+      } else if (parameter.as_double() < 0.0) {
+        RCLCPP_WARN(
+          logger_, "The value of parameter '%s' is incorrectly set to %f, "
+          "it should be >=0. Ignoring parameter update.",
+          name.c_str(), parameter.as_double());
+        result.successful = false;
+      }
+    } else if (type == ParameterType::PARAMETER_BOOL) {
+      if (name == plugin_name_ + ".allow_reversing") {
+        if (params_.use_rotate_to_heading && parameter.as_bool()) {
           RCLCPP_WARN(
-            logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
-            "it should be >0. Ignoring parameter update.");
-          continue;
+            logger_, "Both use_rotate_to_heading and allow_reversing "
+            "parameter cannot be set to true. Rejecting parameter update.");
+          result.successful = false;
         }
+      }
+    }
+  }
+  return result;
+}
+void
+ParameterHandler::updateParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  std::lock_guard<std::mutex> lock_reinit(mutex_);
+
+  for (const auto & parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    if (type == ParameterType::PARAMETER_DOUBLE) {
+      if (name == plugin_name_ + ".inflation_cost_scaling_factor") {
         params_.inflation_cost_scaling_factor = parameter.as_double();
       } else if (name == plugin_name_ + ".desired_linear_vel") {
         params_.desired_linear_vel = parameter.as_double();
@@ -257,6 +292,10 @@ ParameterHandler::dynamicParametersCallback(
         params_.cancel_deceleration = parameter.as_double();
       } else if (name == plugin_name_ + ".rotate_to_heading_min_angle") {
         params_.rotate_to_heading_min_angle = parameter.as_double();
+      } else if (name == plugin_name_ + ".transform_tolerance") {
+        params_.transform_tolerance = parameter.as_double();
+      } else if (name == plugin_name_ + ".max_robot_pose_search_dist") {
+        params_.max_robot_pose_search_dist = parameter.as_double();
       }
     } else if (type == ParameterType::PARAMETER_BOOL) {
       if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") {
@@ -274,19 +313,12 @@ ParameterHandler::dynamicParametersCallback(
       } else if (name == plugin_name_ + ".use_cancel_deceleration") {
         params_.use_cancel_deceleration = parameter.as_bool();
       } else if (name == plugin_name_ + ".allow_reversing") {
-        if (params_.use_rotate_to_heading && parameter.as_bool()) {
-          RCLCPP_WARN(
-            logger_, "Both use_rotate_to_heading and allow_reversing "
-            "parameter cannot be set to true. Rejecting parameter update.");
-          continue;
-        }
         params_.allow_reversing = parameter.as_bool();
+      } else if (name == plugin_name_ + ".interpolate_curvature_after_goal") {
+        params_.interpolate_curvature_after_goal = parameter.as_bool();
       }
     }
   }
-
-  result.successful = true;
-  return result;
 }
 
 }  // namespace nav2_regulated_pure_pursuit_controller
diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
index 45a8aa274c4..a46f0e741f1 100644
--- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
+++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
@@ -1116,7 +1116,7 @@ TEST_F(TransformGlobalPlanTest, prune_after_leaving_costmap)
   EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_rotation_shim_controller/README.md b/nav2_rotation_shim_controller/README.md
index 675a2aefe3c..3a19898a40a 100644
--- a/nav2_rotation_shim_controller/README.md
+++ b/nav2_rotation_shim_controller/README.md
@@ -1,8 +1,8 @@
 # Nav2 Rotation Shim Controller
 
-This is a controller (local trajectory planner) that implements a "shim" controller plugin. It was developed by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). 
+This is a controller (local trajectory planner) that implements a "shim" controller plugin. It was developed by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/).
 
-The Rotation Shim Controller stands between the controller server and the main controller plugin to implement a specific behavior often troublesome for other algorithms. This shim will rotate a robot in place to the rough heading of a newly received path. Afterwards, it will forward all future commands on that path to the main controller. It will take in a ``primary_controller`` parameter, containing the actual controller to use for path tracking once aligned with the path. 
+The Rotation Shim Controller stands between the controller server and the main controller plugin to implement a specific behavior often troublesome for other algorithms. This shim will rotate a robot in place to the rough heading of a newly received path. Afterwards, it will forward all future commands on that path to the main controller. It will take in a ``primary_controller`` parameter, containing the actual controller to use for path tracking once aligned with the path.
 
 This is useful for situations when working with plugins that are either too specialized or tuned for a particular task that they can fail to adequately solve the full local planning problem performantly. Examples include:
 
@@ -17,7 +17,7 @@ When the `rotate_to_goal_heading` parameter is set to true, this controller is a
 The Rotation Shim Controller is suitable for:
 - Robots that can rotate in place, such as differential and omnidirectional robots.
 - Preference to rotate in place rather than 'spiral out' when starting to track a new path that is at a significantly different heading than the robot's current heading.
-- Using planners that are non-kinematically feasible, such as NavFn, Theta\*, or Smac 2D (Feasible planners such as Smac Hybrid-A* and State Lattice will start search from the robot's actual starting heading, requiring no rotation). 
+- Using planners that are non-kinematically feasible, such as NavFn, Theta\*, or Smac 2D (Feasible planners such as Smac Hybrid-A* and State Lattice will start search from the robot's actual starting heading, requiring no rotation).
 
 This plugin implements the `nav2_core::Controller` interface allowing it to be used across the navigation stack as a local trajectory planner in the controller server's action server (`controller_server`). It will host an internal plugin of your actual path tracker (e.g. MPPI, RPP, DWB, TEB, etc) that will be used after the robot has rotated to the rough starting heading of the path.
 
@@ -29,15 +29,16 @@ See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/
 
 ## Configuration
 
-| Parameter | Description | 
+| Parameter | Description |
 |-----|----|
-| `angular_dist_threshold` | Maximum angular distance, in radians, away from the path heading to trigger rotation until within. | 
-| `forward_sampling_distance` | Forward distance, in meters, along path to select a sampling point to use to approximate path heading | 
-| `rotate_to_heading_angular_vel` | Angular rotational velocity, in rad/s, to rotate to the path heading | 
-| `primary_controller` | Internal controller plugin to use for actual control behavior after rotating to heading | 
-| `max_angular_accel` | Maximum angular acceleration for rotation to heading | 
-| `simulate_ahead_time` | Time in seconds to forward simulate a rotation command to check for collisions. If a collision is found, forwards control back to the primary controller plugin. | 
-| `rotate_to_goal_heading` | If true, the rotationShimController will take back control of the robot when in XY tolerance of the goal and start rotating to the goal heading | 
+| `angular_dist_threshold` | Maximum angular distance, in radians, away from the path heading to trigger rotation until within. |
+| `forward_sampling_distance` | Forward distance, in meters, along path to select a sampling point to use to approximate path heading |
+| `rotate_to_heading_angular_vel` | Angular rotational velocity, in rad/s, to rotate to the path heading |
+| `primary_controller` | Internal controller plugin to use for actual control behavior after rotating to heading |
+| `max_angular_accel` | Maximum angular acceleration for rotation to heading |
+| `simulate_ahead_time` | Time in seconds to forward simulate a rotation command to check for collisions. If a collision is found, forwards control back to the primary controller plugin. |
+| `rotate_to_goal_heading` | If true, the rotationShimController will take back control of the robot when in XY tolerance of the goal and start rotating to the goal heading |
+| `use_path_orientations` | If true, the controller will use the orientations of the path points to compute the heading of the path instead of computing the heading from the path points. If true, the controller will use the orientations of the path points to compute the heading of the path instead of computing the heading from the path points. Use for for feasible planners like the Smac Planner which generate feasible paths with orientations for forward and reverse motion. |
 
 Example fully-described XML with default parameter values:
 
@@ -70,6 +71,7 @@ controller_server:
       max_angular_accel: 3.2
       simulate_ahead_time: 1.0
       rotate_to_goal_heading: false
+      use_path_orientations: false
 
       # DWB parameters
       ...
diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp
index 1a13eb00c1d..b587c4defd2 100644
--- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp
+++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp
@@ -186,6 +186,7 @@ class RotationShimController : public nav2_core::Controller
   double control_duration_, simulate_ahead_time_;
   bool rotate_to_goal_heading_, in_rotation_, rotate_to_heading_once_;
   bool closed_loop_;
+  bool use_path_orientations_;
   double last_angular_vel_ = std::numeric_limits<double>::max();
 
   // Dynamic parameters handler
diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
index 1d8e3d5da6b..358600e935f 100644
--- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
+++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
@@ -75,6 +75,8 @@ void RotationShimController::configure(
     node, plugin_name_ + ".rotate_to_heading_once", rclcpp::ParameterValue(false));
   nav2_util::declare_parameter_if_not_declared(
     node, plugin_name_ + ".closed_loop", rclcpp::ParameterValue(true));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name_ + ".use_path_orientations", rclcpp::ParameterValue(false));
 
   node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_);
   node->get_parameter(plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_);
@@ -92,6 +94,7 @@ void RotationShimController::configure(
   node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_);
   node->get_parameter(plugin_name_ + ".rotate_to_heading_once", rotate_to_heading_once_);
   node->get_parameter(plugin_name_ + ".closed_loop", closed_loop_);
+  node->get_parameter(plugin_name_ + ".use_path_orientations", use_path_orientations_);
 
   try {
     primary_controller_ = lp_loader_.createUniqueInstance(primary_controller);
@@ -207,10 +210,18 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands
 
     std::lock_guard<std::mutex> lock_reinit(mutex_);
     try {
-      geometry_msgs::msg::Pose sampled_pt_base = transformPoseToBaseFrame(getSampledPathPt());
-
-      double angular_distance_to_heading =
-        std::atan2(sampled_pt_base.position.y, sampled_pt_base.position.x);
+      auto sampled_pt = getSampledPathPt();
+      double angular_distance_to_heading;
+      if (use_path_orientations_) {
+        angular_distance_to_heading = angles::shortest_angular_distance(
+          tf2::getYaw(pose.pose.orientation),
+          tf2::getYaw(sampled_pt.pose.orientation));
+      } else {
+        geometry_msgs::msg::Pose sampled_pt_base = transformPoseToBaseFrame(sampled_pt);
+        angular_distance_to_heading = std::atan2(
+          sampled_pt_base.position.y,
+          sampled_pt_base.position.x);
+      }
 
       double angular_thresh =
         in_rotation_ ? angular_disengage_threshold_ : angular_dist_threshold_;
@@ -423,6 +434,8 @@ RotationShimController::dynamicParametersCallback(std::vector<rclcpp::Parameter>
         rotate_to_heading_once_ = parameter.as_bool();
       } else if (name == plugin_name_ + ".closed_loop") {
         closed_loop_ = parameter.as_bool();
+      } else if (name == plugin_name_ + ".use_path_orientations") {
+        use_path_orientations_ = parameter.as_bool();
       }
     }
   }
diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp
index 805a3a37ca1..6bd7838017a 100644
--- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp
+++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp
@@ -621,7 +621,8 @@ TEST(RotationShimControllerTest, testDynamicParameter)
       rclcpp::Parameter("test.primary_controller", std::string("HI")),
       rclcpp::Parameter("test.rotate_to_goal_heading", true),
       rclcpp::Parameter("test.rotate_to_heading_once", true),
-      rclcpp::Parameter("test.closed_loop", false)});
+      rclcpp::Parameter("test.closed_loop", false),
+      rclcpp::Parameter("test.use_path_orientations", true)});
 
   rclcpp::spin_until_future_complete(
     node->get_node_base_interface(),
@@ -635,9 +636,10 @@ TEST(RotationShimControllerTest, testDynamicParameter)
   EXPECT_EQ(node->get_parameter("test.rotate_to_goal_heading").as_bool(), true);
   EXPECT_EQ(node->get_parameter("test.rotate_to_heading_once").as_bool(), true);
   EXPECT_EQ(node->get_parameter("test.closed_loop").as_bool(), false);
+  EXPECT_EQ(node->get_parameter("test.use_path_orientations").as_bool(), true);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp
index 6b57eee6170..837201ea231 100644
--- a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp
+++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp
@@ -23,6 +23,7 @@
 #include <rviz_common/properties/bool_property.hpp>
 #include <rviz_common/properties/qos_profile_property.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include "nav2_util/service_client.hpp"
 
 namespace nav2_rviz_plugins
 {
@@ -48,8 +49,8 @@ class CostmapCostTool : public rviz_common::Tool
 private Q_SLOTS:
 
 private:
-  rclcpp::Client<nav2_msgs::srv::GetCosts>::SharedPtr local_cost_client_;
-  rclcpp::Client<nav2_msgs::srv::GetCosts>::SharedPtr global_cost_client_;
+  nav2_util::ServiceClient<nav2_msgs::srv::GetCosts>::SharedPtr local_cost_client_;
+  nav2_util::ServiceClient<nav2_msgs::srv::GetCosts>::SharedPtr global_cost_client_;
   // The Node pointer that we need to keep alive for the duration of this plugin.
   std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_ptr_;
 
diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp
index 03435a913e4..47539607ebb 100644
--- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp
+++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp
@@ -30,7 +30,7 @@ CostmapCostTool::CostmapCostTool()
   auto_deactivate_property_ = new rviz_common::properties::BoolProperty(
     "Single click", true,
     "Switch away from this tool after one click.",
-    getPropertyContainer(), SLOT(updateAutoDeactivate()), this);
+    getPropertyContainer(), nullptr, this);
 }
 
 CostmapCostTool::~CostmapCostTool() {}
@@ -52,11 +52,16 @@ void CostmapCostTool::onInitialize()
     return;
   }
   rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
-
   local_cost_client_ =
-    node->create_client<nav2_msgs::srv::GetCosts>("/local_costmap/get_cost_local_costmap");
+    std::make_shared<nav2_util::ServiceClient<nav2_msgs::srv::GetCosts>>(
+    "/local_costmap/get_cost_local_costmap",
+    node,
+    false /* Does not create and spin an internal executor*/);
   global_cost_client_ =
-    node->create_client<nav2_msgs::srv::GetCosts>("/global_costmap/get_cost_global_costmap");
+    std::make_shared<nav2_util::ServiceClient<nav2_msgs::srv::GetCosts>>(
+    "/global_costmap/get_cost_global_costmap",
+    node,
+    false /* Does not create and spin an internal executor*/);
 }
 
 void CostmapCostTool::activate() {}
@@ -107,14 +112,14 @@ void CostmapCostTool::callCostService(float x, float y)
 
   // Call local costmap service
   if (local_cost_client_->wait_for_service(std::chrono::seconds(1))) {
-    local_cost_client_->async_send_request(
+    local_cost_client_->async_call(
       request,
       std::bind(&CostmapCostTool::handleLocalCostResponse, this, std::placeholders::_1));
   }
 
   // Call global costmap service
   if (global_cost_client_->wait_for_service(std::chrono::seconds(1))) {
-    global_cost_client_->async_send_request(
+    global_cost_client_->async_call(
       request,
       std::bind(&CostmapCostTool::handleGlobalCostResponse, this, std::placeholders::_1));
   }
diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp
index 0b5ee1055ff..c8a977730ee 100644
--- a/nav2_rviz_plugins/src/docking_panel.cpp
+++ b/nav2_rviz_plugins/src/docking_panel.cpp
@@ -252,7 +252,7 @@ DockingPanel::DockingPanel(QWidget * parent)
       if (!plugins_loaded_) {
         RCLCPP_INFO(client_node_->get_logger(), "Loading dock plugins");
         nav2_rviz_plugins::pluginLoader(
-        client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_);
+          client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_);
         plugins_loaded_ = true;
       }
     });
diff --git a/nav2_rviz_plugins/src/selector.cpp b/nav2_rviz_plugins/src/selector.cpp
index 086554573d4..1fffd8ff425 100644
--- a/nav2_rviz_plugins/src/selector.cpp
+++ b/nav2_rviz_plugins/src/selector.cpp
@@ -149,35 +149,36 @@ void Selector::setProgressChecker()
 void
 Selector::loadPlugins()
 {
-  load_plugins_thread_ = std::thread([this]() {
-        rclcpp::Rate rate(0.2);
-        while (rclcpp::ok() && !plugins_loaded_) {
-          RCLCPP_INFO(client_node_->get_logger(), "Trying to load plugins...");
-          nav2_rviz_plugins::pluginLoader(
-            client_node_, server_failed_, "controller_server", "controller_plugins", controller_);
-          nav2_rviz_plugins::pluginLoader(
-            client_node_, server_failed_, "planner_server", "planner_plugins", planner_);
-          nav2_rviz_plugins::pluginLoader(
-            client_node_, server_failed_, "controller_server", "goal_checker_plugins",
-            goal_checker_);
-          nav2_rviz_plugins::pluginLoader(
-            client_node_, server_failed_, "smoother_server", "smoother_plugins", smoother_);
-          nav2_rviz_plugins::pluginLoader(
-            client_node_, server_failed_, "controller_server", "progress_checker_plugins",
-            progress_checker_);
-          if (controller_->count() > 0 &&
-          planner_->count() > 0 &&
-          goal_checker_->count() > 0 &&
-          smoother_->count() > 0 &&
-          progress_checker_->count() > 0)
-          {
-            plugins_loaded_ = true;
-          } else {
-            RCLCPP_INFO(client_node_->get_logger(), "Failed to load plugins. Retrying...");
-          }
-          rate.sleep();
+  load_plugins_thread_ = std::thread(
+    [this]() {
+      rclcpp::Rate rate(0.2);
+      while (rclcpp::ok() && !plugins_loaded_) {
+        RCLCPP_INFO(client_node_->get_logger(), "Trying to load plugins...");
+        nav2_rviz_plugins::pluginLoader(
+          client_node_, server_failed_, "controller_server", "controller_plugins", controller_);
+        nav2_rviz_plugins::pluginLoader(
+          client_node_, server_failed_, "planner_server", "planner_plugins", planner_);
+        nav2_rviz_plugins::pluginLoader(
+          client_node_, server_failed_, "controller_server", "goal_checker_plugins",
+          goal_checker_);
+        nav2_rviz_plugins::pluginLoader(
+          client_node_, server_failed_, "smoother_server", "smoother_plugins", smoother_);
+        nav2_rviz_plugins::pluginLoader(
+          client_node_, server_failed_, "controller_server", "progress_checker_plugins",
+          progress_checker_);
+        if (controller_->count() > 0 &&
+        planner_->count() > 0 &&
+        goal_checker_->count() > 0 &&
+        smoother_->count() > 0 &&
+        progress_checker_->count() > 0)
+        {
+          plugins_loaded_ = true;
+        } else {
+          RCLCPP_INFO(client_node_->get_logger(), "Failed to load plugins. Retrying...");
         }
-  });
+        rate.sleep();
+      }
+    });
 }
 
 }  // namespace nav2_rviz_plugins
diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md
index 2b900437fec..f13cf8491bf 100644
--- a/nav2_simple_commander/README.md
+++ b/nav2_simple_commander/README.md
@@ -118,6 +118,6 @@ The `nav2_simple_commander` has a few examples to highlight the API functions av
 
 The `nav2_simple_commander` has a few demonstrations to highlight a couple of simple autonomy applications you can build using the `nav2_simple_commander` API:
 
-- `demo_security.py` - A simple security robot application, showing how to have a robot follow a security route using Navigate Through Poses to do a patrol route, indefinitely. 
+- `demo_security.py` - A simple security robot application, showing how to have a robot follow a security route using Navigate Through Poses to do a patrol route, indefinitely.
 - `demo_picking.py` - A simple item picking application, showing how to have a robot drive to a specific shelf in a warehouse to either pick an item or have a person place an item into a basket and deliver it to a destination for shipping using Navigate To Pose.
 - `demo_inspection.py` - A simple shelf inspection application, showing how to use the Waypoint Follower and task executors to take pictures, RFID scans, etc of shelves to analyze the current shelf statuses and locate items in the warehouse.
diff --git a/nav2_simple_commander/launch/assisted_teleop_example_launch.py b/nav2_simple_commander/launch/assisted_teleop_example_launch.py
index ad3119ce441..756ad7d5946 100644
--- a/nav2_simple_commander/launch/assisted_teleop_example_launch.py
+++ b/nav2_simple_commander/launch/assisted_teleop_example_launch.py
@@ -18,16 +18,9 @@
 import tempfile
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
+from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
+                            IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler)
 from launch.conditions import IfCondition
 from launch.event_handlers import OnShutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -35,7 +28,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')
diff --git a/nav2_simple_commander/launch/follow_path_example_launch.py b/nav2_simple_commander/launch/follow_path_example_launch.py
index b07fef9ba68..fc205c27016 100644
--- a/nav2_simple_commander/launch/follow_path_example_launch.py
+++ b/nav2_simple_commander/launch/follow_path_example_launch.py
@@ -17,16 +17,9 @@
 import tempfile
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
+from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
+                            IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler)
 from launch.conditions import IfCondition
 from launch.event_handlers import OnShutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -34,7 +27,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')
diff --git a/nav2_simple_commander/launch/inspection_demo_launch.py b/nav2_simple_commander/launch/inspection_demo_launch.py
index be98c14150d..01f3443cc22 100644
--- a/nav2_simple_commander/launch/inspection_demo_launch.py
+++ b/nav2_simple_commander/launch/inspection_demo_launch.py
@@ -17,16 +17,9 @@
 import tempfile
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
+from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
+                            IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler)
 from launch.conditions import IfCondition
 from launch.event_handlers import OnShutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -34,7 +27,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')
diff --git a/nav2_simple_commander/launch/nav_through_poses_example_launch.py b/nav2_simple_commander/launch/nav_through_poses_example_launch.py
index 593ee1e8816..c4a73c4887a 100644
--- a/nav2_simple_commander/launch/nav_through_poses_example_launch.py
+++ b/nav2_simple_commander/launch/nav_through_poses_example_launch.py
@@ -17,16 +17,9 @@
 import tempfile
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
+from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
+                            IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler)
 from launch.conditions import IfCondition
 from launch.event_handlers import OnShutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -34,7 +27,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')
diff --git a/nav2_simple_commander/launch/nav_to_pose_example_launch.py b/nav2_simple_commander/launch/nav_to_pose_example_launch.py
index c3e22fb08d3..832ed7cdce7 100644
--- a/nav2_simple_commander/launch/nav_to_pose_example_launch.py
+++ b/nav2_simple_commander/launch/nav_to_pose_example_launch.py
@@ -17,16 +17,9 @@
 import tempfile
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
+from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
+                            IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler)
 from launch.conditions import IfCondition
 from launch.event_handlers import OnShutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -34,7 +27,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')
diff --git a/nav2_simple_commander/launch/picking_demo_launch.py b/nav2_simple_commander/launch/picking_demo_launch.py
index f4bd8346013..41f0180ad69 100644
--- a/nav2_simple_commander/launch/picking_demo_launch.py
+++ b/nav2_simple_commander/launch/picking_demo_launch.py
@@ -17,16 +17,9 @@
 import tempfile
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
+from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
+                            IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler)
 from launch.conditions import IfCondition
 from launch.event_handlers import OnShutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -34,7 +27,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')
diff --git a/nav2_simple_commander/launch/recoveries_example_launch.py b/nav2_simple_commander/launch/recoveries_example_launch.py
index c7afd73f31c..7c94600edfd 100644
--- a/nav2_simple_commander/launch/recoveries_example_launch.py
+++ b/nav2_simple_commander/launch/recoveries_example_launch.py
@@ -17,16 +17,9 @@
 import tempfile
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
+from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
+                            IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler)
 from launch.conditions import IfCondition
 from launch.event_handlers import OnShutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -34,7 +27,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')
diff --git a/nav2_simple_commander/launch/security_demo_launch.py b/nav2_simple_commander/launch/security_demo_launch.py
index 240f430b236..e7ae653e0f0 100644
--- a/nav2_simple_commander/launch/security_demo_launch.py
+++ b/nav2_simple_commander/launch/security_demo_launch.py
@@ -17,16 +17,9 @@
 import tempfile
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
+from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
+                            IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler)
 from launch.conditions import IfCondition
 from launch.event_handlers import OnShutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -34,7 +27,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')
diff --git a/nav2_simple_commander/launch/waypoint_follower_example_launch.py b/nav2_simple_commander/launch/waypoint_follower_example_launch.py
index 10f36ce7922..22952fc6739 100644
--- a/nav2_simple_commander/launch/waypoint_follower_example_launch.py
+++ b/nav2_simple_commander/launch/waypoint_follower_example_launch.py
@@ -17,16 +17,9 @@
 import tempfile
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
+from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
+                            IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler)
 from launch.conditions import IfCondition
 from launch.event_handlers import OnShutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -34,7 +27,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')
diff --git a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py
index ad4014522aa..630af757bfd 100644
--- a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py
+++ b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py
@@ -23,7 +23,12 @@
 and handling semantics found in the costmap 2d C++ API.
 """
 
+from typing import Optional
+
+from builtin_interfaces.msg import Time
+from nav_msgs.msg import OccupancyGrid
 import numpy as np
+from numpy.typing import NDArray
 
 
 class PyCostmap2D:
@@ -33,7 +38,7 @@ class PyCostmap2D:
     Costmap Python3 API for OccupancyGrids to populate from published messages
     """
 
-    def __init__(self, occupancy_map):
+    def __init__(self, occupancy_map: OccupancyGrid) -> None:
         """
         Initialize costmap2D.
 
@@ -46,49 +51,49 @@ def __init__(self, occupancy_map):
             None
 
         """
-        self.size_x = occupancy_map.info.width
-        self.size_y = occupancy_map.info.height
-        self.resolution = occupancy_map.info.resolution
-        self.origin_x = occupancy_map.info.origin.position.x
-        self.origin_y = occupancy_map.info.origin.position.y
-        self.global_frame_id = occupancy_map.header.frame_id
-        self.costmap_timestamp = occupancy_map.header.stamp
+        self.size_x: int = occupancy_map.info.width
+        self.size_y: int = occupancy_map.info.height
+        self.resolution: float = occupancy_map.info.resolution
+        self.origin_x: float = occupancy_map.info.origin.position.x
+        self.origin_y: float = occupancy_map.info.origin.position.y
+        self.global_frame_id: str = occupancy_map.header.frame_id
+        self.costmap_timestamp: Time = occupancy_map.header.stamp
         # Extract costmap
-        self.costmap = np.array(occupancy_map.data, dtype=np.uint8)
+        self.costmap: NDArray[np.uint8] = np.array(occupancy_map.data, dtype=np.uint8)
 
-    def getSizeInCellsX(self):
+    def getSizeInCellsX(self) -> int:
         """Get map width in cells."""
         return self.size_x
 
-    def getSizeInCellsY(self):
+    def getSizeInCellsY(self) -> int:
         """Get map height in cells."""
         return self.size_y
 
-    def getSizeInMetersX(self):
+    def getSizeInMetersX(self) -> float:
         """Get x axis map size in meters."""
         return (self.size_x - 1 + 0.5) * self.resolution
 
-    def getSizeInMetersY(self):
+    def getSizeInMetersY(self) -> float:
         """Get y axis map size in meters."""
         return (self.size_y - 1 + 0.5) * self.resolution
 
-    def getOriginX(self):
+    def getOriginX(self) -> float:
         """Get the origin x axis of the map [m]."""
         return self.origin_x
 
-    def getOriginY(self):
+    def getOriginY(self) -> float:
         """Get the origin y axis of the map [m]."""
         return self.origin_y
 
-    def getResolution(self):
+    def getResolution(self) -> float:
         """Get map resolution [m/cell]."""
         return self.resolution
 
-    def getGlobalFrameID(self):
+    def getGlobalFrameID(self) -> str:
         """Get global frame_id."""
         return self.global_frame_id
 
-    def getCostmapTimestamp(self):
+    def getCostmapTimestamp(self) -> Time:
         """Get costmap timestamp."""
         return self.costmap_timestamp
 
@@ -106,7 +111,7 @@ def getCostXY(self, mx: int, my: int) -> np.uint8:
             np.uint8: cost of a cell
 
         """
-        return self.costmap[self.getIndex(mx, my)]
+        return np.uint8(self.costmap[self.getIndex(mx, my)])
 
     def getCostIdx(self, index: int) -> np.uint8:
         """
@@ -121,7 +126,7 @@ def getCostIdx(self, index: int) -> np.uint8:
             np.uint8: cost of a cell
 
         """
-        return self.costmap[index]
+        return np.uint8(self.costmap[index])
 
     def setCost(self, mx: int, my: int, cost: np.uint8) -> None:
         """
@@ -160,7 +165,7 @@ def mapToWorld(self, mx: int, my: int) -> tuple[float, float]:
         wy = self.origin_y + (my + 0.5) * self.resolution
         return (wx, wy)
 
-    def worldToMapValidated(self, wx: float, wy: float):
+    def worldToMapValidated(self, wx: float, wy: float) -> tuple[Optional[int], Optional[int]]:
         """
         Get the map coordinate XY using world coordinate XY.
 
diff --git a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py
index 6fee029cd8b..f2acdfe8cb1 100644
--- a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py
+++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py
@@ -26,7 +26,7 @@
 """
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()
diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py
index 6f2951d012a..c8fe7301272 100644
--- a/nav2_simple_commander/nav2_simple_commander/demo_picking.py
+++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py
@@ -15,10 +15,15 @@
 
 from geometry_msgs.msg import PoseStamped
 from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
-
 import rclpy
 from rclpy.duration import Duration
 
+"""
+Basic item picking demo. In this demonstration, the expectation
+is that there is a person at the item shelf to put the item on the robot
+and at the pallet jack to remove it
+(probably with some kind of button for 'got item, robot go do next task').
+"""
 
 # Shelf positions for picking
 shelf_positions = {
@@ -35,15 +40,8 @@
     'frieght_bay_3': [-6.0, -5.0, 3.14],
 }
 
-"""
-Basic item picking demo. In this demonstration, the expectation
-is that there is a person at the item shelf to put the item on the robot
-and at the pallet jack to remove it
-(probably with some kind of button for 'got item, robot go do next task').
-"""
-
 
-def main():
+def main() -> None:
     # Received virtual request for picking item at Shelf A and bring to
     # worker at the pallet jack 7 for shipping. This request would
     # contain the shelf ID ('shelf_A') and shipping destination ('frieght_bay_3')
@@ -96,7 +94,7 @@ def main():
                 'Estimated time of arrival at '
                 + request_item_location
                 + ' for worker: '
-                + '{0:.0f}'.format(
+                + '{:.0f}'.format(
                     Duration.from_msg(feedback.estimated_time_remaining).nanoseconds
                     / 1e9
                 )
diff --git a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py
index 112fb30a1bf..dcc3904ee4f 100644
--- a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py
+++ b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py
@@ -18,7 +18,6 @@
 import rclpy
 from rclpy.duration import Duration
 
-
 """
 Basic recoveries demo. In this demonstration, the robot navigates
 to a dead-end where recoveries such as backup and spin are used
@@ -26,7 +25,7 @@
 """
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()
@@ -64,7 +63,7 @@ def main():
             )
 
     # Robot hit a dead end, back it up
-    print('Robot hit a dead end (lets pretend), backing up...')
+    print("Robot hit a dead end (let's pretend), backing up...")
     navigator.backup(backup_dist=0.5, backup_speed=0.1)
 
     i = 0
diff --git a/nav2_simple_commander/nav2_simple_commander/demo_security.py b/nav2_simple_commander/nav2_simple_commander/demo_security.py
index 1c483363e75..fa4be0761e8 100644
--- a/nav2_simple_commander/nav2_simple_commander/demo_security.py
+++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py
@@ -17,11 +17,9 @@
 
 from geometry_msgs.msg import PoseStamped
 from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
-
 import rclpy
 from rclpy.duration import Duration
 
-
 """
 Basic security route patrol demo. In this demonstration, the expectation
 is that there are security cameras mounted on the robots recording or being
@@ -29,7 +27,7 @@
 """
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()
@@ -58,7 +56,7 @@ def main():
     navigator.waitUntilNav2Active()
 
     # Do security route until dead
-    while rclpy.ok():
+    while rclpy.ok():  # type: ignore[attr-defined]
         # Send our route
         route_poses = []
         pose = PoseStamped()
@@ -80,7 +78,7 @@ def main():
             if feedback and i % 5 == 0:
                 print(
                     'Estimated time to complete current route: '
-                    + '{0:.0f}'.format(
+                    + '{:.0f}'.format(
                         Duration.from_msg(feedback.estimated_time_remaining).nanoseconds
                         / 1e9
                     )
diff --git a/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py b/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py
index 0c6a0f58caf..be80705a85a 100644
--- a/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py
+++ b/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py
@@ -24,7 +24,7 @@
 """
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()
diff --git a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py
index 15769a05cfb..9b4439e52f6 100644
--- a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py
+++ b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py
@@ -17,13 +17,12 @@
 from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
 import rclpy
 
-
 """
 Basic navigation demo to follow a given path after smoothing
 """
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()
@@ -70,9 +69,9 @@ def main():
         if feedback and i % 5 == 0:
             print(
                 'Estimated distance remaining to goal position: '
-                + '{0:.3f}'.format(feedback.distance_to_goal)
+                + f'{feedback.distance_to_goal:.3f}'
                 + '\nCurrent speed of the robot: '
-                + '{0:.3f}'.format(feedback.speed)
+                + f'{feedback.speed:.3f}'
             )
 
     # Do something depending on the return code
diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py
index cbcb4e3926d..7585093fd74 100644
--- a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py
+++ b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py
@@ -23,7 +23,7 @@
 """
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()
@@ -102,7 +102,7 @@ def main():
         if feedback and i % 5 == 0:
             print(
                 'Estimated time of arrival: '
-                + '{0:.0f}'.format(
+                + '{:.0f}'.format(
                     Duration.from_msg(feedback.estimated_time_remaining).nanoseconds
                     / 1e9
                 )
diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py
index 5846823440c..985230915a3 100644
--- a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py
+++ b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py
@@ -23,7 +23,7 @@
 """
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()
@@ -82,7 +82,7 @@ def main():
         if feedback and i % 5 == 0:
             print(
                 'Estimated time of arrival: '
-                + '{0:.0f}'.format(
+                + '{:.0f}'.format(
                     Duration.from_msg(feedback.estimated_time_remaining).nanoseconds
                     / 1e9
                 )
diff --git a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py
index 0cf065a7817..a70929b1cd9 100644
--- a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py
+++ b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py
@@ -23,7 +23,7 @@
 """
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()
diff --git a/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py b/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py
index 5cf2f81794a..f721b5f1528 100644
--- a/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py
+++ b/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py
@@ -26,6 +26,7 @@
 from geometry_msgs.msg import Point32, Polygon
 from nav2_simple_commander.costmap_2d import PyCostmap2D
 from nav2_simple_commander.line_iterator import LineIterator
+import numpy as np
 
 NO_INFORMATION = 255
 LETHAL_OBSTACLE = 254
@@ -42,12 +43,12 @@ class FootprintCollisionChecker:
     and checking the collisions of a Footprint
     """
 
-    def __init__(self):
+    def __init__(self) -> None:
         """Initialize the FootprintCollisionChecker Object."""
         self.costmap_ = None
         pass
 
-    def footprintCost(self, footprint: Polygon):
+    def footprintCost(self, footprint: Polygon) -> float:
         """
         Iterate over all the points in a footprint and check for collision.
 
@@ -90,7 +91,8 @@ def footprintCost(self, footprint: Polygon):
 
         return max(float(self.lineCost(xstart, x1, ystart, y1)), footprint_cost)
 
-    def lineCost(self, x0, x1, y0, y1, step_size=0.5):
+    def lineCost(self, x0: float, x1: float,
+                 y0: float, y1: float, step_size: float = 0.5) -> float:
         """
         Iterate over all the points along a line and check for collision.
 
@@ -113,9 +115,9 @@ def lineCost(self, x0, x1, y0, y1, step_size=0.5):
         line_iterator = LineIterator(x0, y0, x1, y1, step_size)
 
         while line_iterator.isValid():
-            point_cost = self.pointCost(
+            point_cost = float(self.pointCost(
                 int(line_iterator.getX()), int(line_iterator.getY())
-            )
+            ))
 
             if point_cost == LETHAL_OBSTACLE:
                 return point_cost
@@ -127,7 +129,7 @@ def lineCost(self, x0, x1, y0, y1, step_size=0.5):
 
         return line_cost
 
-    def worldToMapValidated(self, wx: float, wy: float):
+    def worldToMapValidated(self, wx: float, wy: float) -> tuple[int, int]:
         """
         Get the map coordinate XY using world coordinate XY.
 
@@ -150,7 +152,7 @@ def worldToMapValidated(self, wx: float, wy: float):
             )
         return self.costmap_.worldToMapValidated(wx, wy)
 
-    def pointCost(self, x: int, y: int):
+    def pointCost(self, x: int, y: int) -> np.uint8:
         """
         Get the cost of a point in the costmap using map coordinates XY.
 
@@ -170,7 +172,7 @@ def pointCost(self, x: int, y: int):
             )
         return self.costmap_.getCostXY(x, y)
 
-    def setCostmap(self, costmap: PyCostmap2D):
+    def setCostmap(self, costmap: PyCostmap2D) -> None:
         """
         Specify which costmap to use.
 
@@ -186,7 +188,8 @@ def setCostmap(self, costmap: PyCostmap2D):
         self.costmap_ = costmap
         return None
 
-    def footprintCostAtPose(self, x: float, y: float, theta: float, footprint: Polygon):
+    def footprintCostAtPose(self, x: float, y: float,
+                            theta: float, footprint: Polygon) -> float:
         """
         Get the cost of a footprint at a specific Pose in map coordinates.
 
diff --git a/nav2_simple_commander/nav2_simple_commander/line_iterator.py b/nav2_simple_commander/nav2_simple_commander/line_iterator.py
index 179c5d1da1b..93dbe77f8ca 100644
--- a/nav2_simple_commander/nav2_simple_commander/line_iterator.py
+++ b/nav2_simple_commander/nav2_simple_commander/line_iterator.py
@@ -31,7 +31,9 @@ class LineIterator:
     LineIterator Python3 API for iterating along the points of a given line
     """
 
-    def __init__(self, x0, y0, x1, y1, step_size=1.0):
+    def __init__(self, x0: float, y0: float,
+                 x1: float, y1: float,
+                 step_size: float = 1.0) -> None:
         """
         Initialize the LineIterator.
 
@@ -91,11 +93,11 @@ def __init__(self, x0, y0, x1, y1, step_size=1.0):
                 'Line has zero length (All 4 points have same coordinates)'
             )
 
-    def isValid(self):
+    def isValid(self) -> bool:
         """Check if line is valid."""
         return self.valid_
 
-    def advance(self):
+    def advance(self) -> None:
         """Advance to the next point in the line."""
         if self.x1_ > self.x0_:
             if self.x_ < self.x1_:
@@ -131,35 +133,35 @@ def advance(self):
             else:
                 self.valid_ = False
 
-    def getX(self):
+    def getX(self) -> float:
         """Get the abscissa of the current point."""
         return self.x_
 
-    def getY(self):
+    def getY(self) -> float:
         """Get the ordinate of the current point."""
         return self.y_
 
-    def getX0(self):
+    def getX0(self) -> float:
         """Get the abscissa of the initial point."""
         return self.x0_
 
-    def getY0(self):
+    def getY0(self) -> float:
         """Get the ordinate of the initial point."""
         return self.y0_
 
-    def getX1(self):
+    def getX1(self) -> float:
         """Get the abscissa of the final point."""
         return self.x1_
 
-    def getY1(self):
+    def getY1(self) -> float:
         """Get the ordinate of the final point."""
         return self.y1_
 
-    def get_line_length(self):
+    def get_line_length(self) -> complex:
         """Get the length of the line."""
         return sqrt(pow(self.x1_ - self.x0_, 2) + pow(self.y1_ - self.y0_, 2))
 
-    def clamp(self, n, min_n, max_n):
+    def clamp(self, n: float, min_n: float, max_n: float) -> float:
         """
         Clamp n to be between min_n and max_n.
 
diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py
index 035465caf09..b65dd7449de 100644
--- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py
+++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py
@@ -16,33 +16,28 @@
 
 from enum import Enum
 import time
+from typing import Any, Optional
 
 from action_msgs.msg import GoalStatus
 from builtin_interfaces.msg import Duration
-from geometry_msgs.msg import Point
-from geometry_msgs.msg import PoseStamped
-from geometry_msgs.msg import PoseWithCovarianceStamped
+from geographic_msgs.msg import GeoPose
+from geometry_msgs.msg import Point, PoseStamped, PoseWithCovarianceStamped
 from lifecycle_msgs.srv import GetState
-from nav2_msgs.action import AssistedTeleop, BackUp, DriveOnHeading, Spin
-from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose
-from nav2_msgs.action import (
-    DockRobot,
-    FollowGPSWaypoints,
-    FollowPath,
-    FollowWaypoints,
-    NavigateThroughPoses,
-    NavigateToPose,
-    UndockRobot,
-)
-from nav2_msgs.action import SmoothPath
-from nav2_msgs.srv import ClearCostmapAroundRobot, ClearCostmapExceptRegion, ClearEntireCostmap
-from nav2_msgs.srv import GetCostmap, LoadMap, ManageLifecycleNodes
+from nav2_msgs.action import (AssistedTeleop, BackUp, ComputePathThroughPoses, ComputePathToPose,
+                              DockRobot, DriveOnHeading, FollowGPSWaypoints, FollowPath,
+                              FollowWaypoints, NavigateThroughPoses, NavigateToPose, SmoothPath,
+                              Spin, UndockRobot)
+from nav2_msgs.srv import (ClearCostmapAroundRobot, ClearCostmapExceptRegion, ClearEntireCostmap,
+                           GetCostmap, LoadMap, ManageLifecycleNodes)
+from nav_msgs.msg import Goals, OccupancyGrid, Path
 import rclpy
-from rclpy.action import ActionClient
+from rclpy.action import ActionClient  # type: ignore[attr-defined]
+from rclpy.action.client import ClientGoalHandle
 from rclpy.duration import Duration as rclpyDuration
 from rclpy.node import Node
-from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy
-from rclpy.qos import QoSProfile, QoSReliabilityPolicy
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
+from rclpy.task import Future
+from rclpy.type_support import GetResultServiceResponse
 
 
 class TaskResult(Enum):
@@ -54,14 +49,15 @@ class TaskResult(Enum):
 
 class BasicNavigator(Node):
 
-    def __init__(self, node_name='basic_navigator', namespace=''):
+    def __init__(self, node_name: str = 'basic_navigator', namespace: str = '') -> None:
         super().__init__(node_name=node_name, namespace=namespace)
         self.initial_pose = PoseStamped()
         self.initial_pose.header.frame_id = 'map'
-        self.goal_handle = None
-        self.result_future = None
-        self.feedback = None
-        self.status = None
+        self.goal_handle: Optional[ClientGoalHandle[Any, Any, Any]] = None
+        self.result_future: \
+            Optional[Future[GetResultServiceResponse[Any]]] = None
+        self.feedback: str = ''
+        self.status: Optional[int] = None
         self.last_action_error_code = 0
         self.last_action_error_msg = ''
 
@@ -130,10 +126,10 @@ def __init__(self, node_name='basic_navigator', namespace=''):
             GetCostmap, 'local_costmap/get_costmap'
         )
 
-    def destroyNode(self):
+    def destroyNode(self) -> None:
         self.destroy_node()
 
-    def destroy_node(self):
+    def destroy_node(self) -> None:
         self.nav_through_poses_client.destroy()
         self.nav_to_pose_client.destroy()
         self.follow_waypoints_client.destroy()
@@ -150,13 +146,13 @@ def destroy_node(self):
         self.undocking_client.destroy()
         super().destroy_node()
 
-    def setInitialPose(self, initial_pose):
+    def setInitialPose(self, initial_pose: PoseStamped) -> None:
         """Set the initial pose to the localization system."""
         self.initial_pose_received = False
         self.initial_pose = initial_pose
         self._setInitialPose()
 
-    def goThroughPoses(self, poses, behavior_tree=''):
+    def goThroughPoses(self, poses: Goals, behavior_tree: str = '') -> bool:
         """Send a `NavThroughPoses` action request."""
         self.clearTaskError()
         self.debug("Waiting for 'NavigateThroughPoses' action server")
@@ -174,7 +170,7 @@ def goThroughPoses(self, poses, behavior_tree=''):
         rclpy.spin_until_future_complete(self, send_goal_future)
         self.goal_handle = send_goal_future.result()
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             msg = f'NavigateThroughPoses request with {len(poses)} was rejected!'
             self.setTaskError(NavigateThroughPoses.UNKNOWN, msg)
             self.error(msg)
@@ -183,7 +179,7 @@ def goThroughPoses(self, poses, behavior_tree=''):
         self.result_future = self.goal_handle.get_result_async()
         return True
 
-    def goToPose(self, pose, behavior_tree=''):
+    def goToPose(self, pose: PoseStamped, behavior_tree: str = '') -> bool:
         """Send a `NavToPose` action request."""
         self.clearTaskError()
         self.debug("Waiting for 'NavigateToPose' action server")
@@ -207,7 +203,7 @@ def goToPose(self, pose, behavior_tree=''):
         rclpy.spin_until_future_complete(self, send_goal_future)
         self.goal_handle = send_goal_future.result()
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             msg = (
                 'NavigateToPose goal to '
                 + str(pose.pose.position.x)
@@ -222,7 +218,7 @@ def goToPose(self, pose, behavior_tree=''):
         self.result_future = self.goal_handle.get_result_async()
         return True
 
-    def followWaypoints(self, poses):
+    def followWaypoints(self, poses: list[PoseStamped]) -> bool:
         """Send a `FollowWaypoints` action request."""
         self.clearTaskError()
         self.debug("Waiting for 'FollowWaypoints' action server")
@@ -239,7 +235,7 @@ def followWaypoints(self, poses):
         rclpy.spin_until_future_complete(self, send_goal_future)
         self.goal_handle = send_goal_future.result()
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             msg = f'Following {len(poses)} waypoints request was rejected!'
             self.setTaskError(FollowWaypoints.UNKNOWN, msg)
             self.error(msg)
@@ -248,7 +244,7 @@ def followWaypoints(self, poses):
         self.result_future = self.goal_handle.get_result_async()
         return True
 
-    def followGpsWaypoints(self, gps_poses):
+    def followGpsWaypoints(self, gps_poses: list[GeoPose]) -> bool:
         """Send a `FollowGPSWaypoints` action request."""
         self.clearTaskError()
         self.debug("Waiting for 'FollowWaypoints' action server")
@@ -265,7 +261,7 @@ def followGpsWaypoints(self, gps_poses):
         rclpy.spin_until_future_complete(self, send_goal_future)
         self.goal_handle = send_goal_future.result()
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             msg = f'Following {len(gps_poses)} gps waypoints request was rejected!'
             self.setTaskError(FollowGPSWaypoints.UNKNOWN, msg)
             self.error(msg)
@@ -274,7 +270,9 @@ def followGpsWaypoints(self, gps_poses):
         self.result_future = self.goal_handle.get_result_async()
         return True
 
-    def spin(self, spin_dist=1.57, time_allowance=10, disable_collision_checks=False):
+    def spin(
+            self, spin_dist: float = 1.57, time_allowance: int = 10,
+            disable_collision_checks: bool = False) -> bool:
         self.clearTaskError()
         self.debug("Waiting for 'Spin' action server")
         while not self.spin_client.wait_for_server(timeout_sec=1.0):
@@ -291,7 +289,7 @@ def spin(self, spin_dist=1.57, time_allowance=10, disable_collision_checks=False
         rclpy.spin_until_future_complete(self, send_goal_future)
         self.goal_handle = send_goal_future.result()
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             msg = 'Spin request was rejected!'
             self.setTaskError(Spin.UNKNOWN, msg)
             self.error(msg)
@@ -300,8 +298,9 @@ def spin(self, spin_dist=1.57, time_allowance=10, disable_collision_checks=False
         self.result_future = self.goal_handle.get_result_async()
         return True
 
-    def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10,
-               disable_collision_checks=False):
+    def backup(
+            self, backup_dist: float = 0.15, backup_speed: float = 0.025,
+            time_allowance: int = 10, disable_collision_checks: bool = False) -> bool:
         self.clearTaskError()
         self.debug("Waiting for 'Backup' action server")
         while not self.backup_client.wait_for_server(timeout_sec=1.0):
@@ -319,7 +318,7 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10,
         rclpy.spin_until_future_complete(self, send_goal_future)
         self.goal_handle = send_goal_future.result()
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             msg = 'Backup request was rejected!'
             self.setTaskError(BackUp.UNKNOWN, msg)
             self.error(msg)
@@ -328,8 +327,9 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10,
         self.result_future = self.goal_handle.get_result_async()
         return True
 
-    def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10,
-                       disable_collision_checks=False):
+    def driveOnHeading(
+            self, dist: float = 0.15, speed: float = 0.025,
+            time_allowance: int = 10, disable_collision_checks: bool = False) -> bool:
         self.clearTaskError()
         self.debug("Waiting for 'DriveOnHeading' action server")
         while not self.backup_client.wait_for_server(timeout_sec=1.0):
@@ -347,7 +347,7 @@ def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10,
         rclpy.spin_until_future_complete(self, send_goal_future)
         self.goal_handle = send_goal_future.result()
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             msg = 'Drive On Heading request was rejected!'
             self.setTaskError(DriveOnHeading.UNKNOWN, msg)
             self.error(msg)
@@ -356,7 +356,7 @@ def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10,
         self.result_future = self.goal_handle.get_result_async()
         return True
 
-    def assistedTeleop(self, time_allowance=30):
+    def assistedTeleop(self, time_allowance: int = 30) -> bool:
 
         self.clearTaskError()
         self.debug("Wanting for 'assisted_teleop' action server")
@@ -373,7 +373,7 @@ def assistedTeleop(self, time_allowance=30):
         rclpy.spin_until_future_complete(self, send_goal_future)
         self.goal_handle = send_goal_future.result()
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             msg = 'Assisted Teleop request was rejected!'
             self.setTaskError(AssistedTeleop.UNKNOWN, msg)
             self.error(msg)
@@ -382,7 +382,8 @@ def assistedTeleop(self, time_allowance=30):
         self.result_future = self.goal_handle.get_result_async()
         return True
 
-    def followPath(self, path, controller_id='', goal_checker_id=''):
+    def followPath(self, path: Path, controller_id: str = '',
+                   goal_checker_id: str = '') -> bool:
         self.clearTaskError()
         """Send a `FollowPath` action request."""
         self.debug("Waiting for 'FollowPath' action server")
@@ -401,7 +402,7 @@ def followPath(self, path, controller_id='', goal_checker_id=''):
         rclpy.spin_until_future_complete(self, send_goal_future)
         self.goal_handle = send_goal_future.result()
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             msg = 'FollowPath goal was rejected!'
             self.setTaskError(FollowPath.UNKNOWN, msg)
             self.error(msg)
@@ -410,7 +411,8 @@ def followPath(self, path, controller_id='', goal_checker_id=''):
         self.result_future = self.goal_handle.get_result_async()
         return True
 
-    def dockRobotByPose(self, dock_pose, dock_type, nav_to_dock=True):
+    def dockRobotByPose(self, dock_pose: PoseStamped,
+                        dock_type: str = '', nav_to_dock: bool = True) -> bool:
         self.clearTaskError()
         """Send a `DockRobot` action request."""
         self.info("Waiting for 'DockRobot' action server")
@@ -429,7 +431,7 @@ def dockRobotByPose(self, dock_pose, dock_type, nav_to_dock=True):
         rclpy.spin_until_future_complete(self, send_goal_future)
         self.goal_handle = send_goal_future.result()
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             msg = 'DockRobot request was rejected!'
             self.setTaskError(DockRobot.UNKNOWN, msg)
             self.error(msg)
@@ -438,7 +440,7 @@ def dockRobotByPose(self, dock_pose, dock_type, nav_to_dock=True):
         self.result_future = self.goal_handle.get_result_async()
         return True
 
-    def dockRobotByID(self, dock_id, nav_to_dock=True):
+    def dockRobotByID(self, dock_id: str, nav_to_dock: bool = True) -> bool:
         """Send a `DockRobot` action request."""
         self.clearTaskError()
         self.info("Waiting for 'DockRobot' action server")
@@ -456,7 +458,7 @@ def dockRobotByID(self, dock_id, nav_to_dock=True):
         rclpy.spin_until_future_complete(self, send_goal_future)
         self.goal_handle = send_goal_future.result()
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             msg = 'DockRobot request was rejected!'
             self.setTaskError(DockRobot.UNKNOWN, msg)
             self.error(msg)
@@ -465,7 +467,7 @@ def dockRobotByID(self, dock_id, nav_to_dock=True):
         self.result_future = self.goal_handle.get_result_async()
         return True
 
-    def undockRobot(self, dock_type=''):
+    def undockRobot(self, dock_type: str = '') -> bool:
         """Send a `UndockRobot` action request."""
         self.clearTaskError()
         self.info("Waiting for 'UndockRobot' action server")
@@ -481,7 +483,7 @@ def undockRobot(self, dock_type=''):
         rclpy.spin_until_future_complete(self, send_goal_future)
         self.goal_handle = send_goal_future.result()
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             msg = 'UndockRobot request was rejected!'
             self.setTaskError(UndockRobot.UNKNOWN, msg)
             self.error(msg)
@@ -490,12 +492,15 @@ def undockRobot(self, dock_type=''):
         self.result_future = self.goal_handle.get_result_async()
         return True
 
-    def cancelTask(self):
+    def cancelTask(self) -> None:
         """Cancel pending task request of any type."""
         self.info('Canceling current task.')
         if self.result_future:
-            future = self.goal_handle.cancel_goal_async()
-            rclpy.spin_until_future_complete(self, future)
+            if self.goal_handle is not None:
+                future = self.goal_handle.cancel_goal_async()
+                rclpy.spin_until_future_complete(self, future)
+            else:
+                self.error('No goal handle to cancel!')
         self.clearTaskError()
         return
 
@@ -505,16 +510,25 @@ def isTaskComplete(self) -> bool:
             # task was cancelled or completed
             return True
         rclpy.spin_until_future_complete(self, self.result_future, timeout_sec=0.10)
-        if self.result_future.result():
-            self.status = self.result_future.result().status
+        result_response = self.result_future.result()
+
+        if result_response:
+            self.status = result_response.status
             if self.status != GoalStatus.STATUS_SUCCEEDED:
-                result = self.result_future.result().result
-                self.setTaskError(result.error_code, result.error_msg)
-                self.debug('Task with failed with'
-                           f' status code:{self.status}'
-                           f' error code:{result.error_code}'
-                           f' error msg:{result.error_msg}')
-                return True
+                result = result_response.result
+                if result is not None:
+                    self.setTaskError(result.error_code, result.error_msg)
+                    self.debug(
+                        'Task with failed with'
+                        f' status code:{self.status}'
+                        f' error code:{result.error_code}'
+                        f' error msg:{result.error_msg}'
+                    )
+                    return True
+                else:
+                    self.setTaskError(0, 'No result received')
+                    self.debug('Task failed with no result received')
+                    return True
         else:
             # Timed out, still processing, not complete yet
             return False
@@ -522,11 +536,11 @@ def isTaskComplete(self) -> bool:
         self.debug('Task succeeded!')
         return True
 
-    def getFeedback(self):
+    def getFeedback(self) -> str:
         """Get the pending action feedback message."""
         return self.feedback
 
-    def getResult(self):
+    def getResult(self) -> TaskResult:
         """Get the pending action result message."""
         if self.status == GoalStatus.STATUS_SUCCEEDED:
             return TaskResult.SUCCEEDED
@@ -537,18 +551,19 @@ def getResult(self):
         else:
             return TaskResult.UNKNOWN
 
-    def clearTaskError(self):
+    def clearTaskError(self) -> None:
         self.last_action_error_code = 0
         self.last_action_error_msg = ''
 
-    def setTaskError(self, error_code, error_msg):
+    def setTaskError(self, error_code: int, error_msg: str) -> None:
         self.last_action_error_code = error_code
         self.last_action_error_msg = error_msg
 
     def getTaskError(self) -> tuple[int, str]:
         return (self.last_action_error_code, self.last_action_error_msg)
 
-    def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'):
+    def waitUntilNav2Active(self, navigator: str = 'bt_navigator',
+                            localizer: str = 'amcl') -> None:
         """Block until the full navigation system is up and running."""
         if localizer != 'robot_localization':  # non-lifecycle node
             self._waitForNodeToActivate(localizer)
@@ -559,8 +574,9 @@ def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'):
         return
 
     def _getPathImpl(
-            self, start, goal, planner_id='', use_start=False
-            ) -> ComputePathToPose.Result:
+        self, start: PoseStamped, goal: PoseStamped,
+        planner_id: str = '', use_start: bool = False
+    ) -> ComputePathToPose.Result:
         """
         Send a `ComputePathToPose` action request.
 
@@ -581,7 +597,7 @@ def _getPathImpl(
         rclpy.spin_until_future_complete(self, send_goal_future)
         self.goal_handle = send_goal_future.result()
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error('Get path was rejected!')
             self.status = GoalStatus.UNKNOWN
             result = ComputePathToPose.Result()
@@ -591,11 +607,13 @@ def _getPathImpl(
 
         self.result_future = self.goal_handle.get_result_async()
         rclpy.spin_until_future_complete(self, self.result_future)
-        self.status = self.result_future.result().status
+        self.status = self.result_future.result().status  # type: ignore[union-attr]
 
-        return self.result_future.result().result
+        return self.result_future.result().result  # type: ignore[union-attr]
 
-    def getPath(self, start, goal, planner_id='', use_start=False):
+    def getPath(
+        self, start: PoseStamped, goal: PoseStamped,
+            planner_id: str = '', use_start: bool = False) -> Path:
         """Send a `ComputePathToPose` action request."""
         self.clearTaskError()
         rtn = self._getPathImpl(start, goal, planner_id, use_start)
@@ -611,7 +629,8 @@ def getPath(self, start, goal, planner_id='', use_start=False):
             return None
 
     def _getPathThroughPosesImpl(
-        self, start, goals, planner_id='', use_start=False
+        self, start: PoseStamped, goals: Goals,
+            planner_id: str = '', use_start: bool = False
     ) -> ComputePathThroughPoses.Result:
         """
         Send a `ComputePathThroughPoses` action request.
@@ -641,7 +660,7 @@ def _getPathThroughPosesImpl(
         rclpy.spin_until_future_complete(self, send_goal_future)
         self.goal_handle = send_goal_future.result()
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error('Get path was rejected!')
             result = ComputePathThroughPoses.Result()
             result.error_code = ComputePathThroughPoses.UNKNOWN
@@ -650,11 +669,13 @@ def _getPathThroughPosesImpl(
 
         self.result_future = self.goal_handle.get_result_async()
         rclpy.spin_until_future_complete(self, self.result_future)
-        self.status = self.result_future.result().status
+        self.status = self.result_future.result().status  # type: ignore[union-attr]
 
-        return self.result_future.result().result
+        return self.result_future.result().result  # type: ignore[union-attr]
 
-    def getPathThroughPoses(self, start, goals, planner_id='', use_start=False):
+    def getPathThroughPoses(
+        self, start: PoseStamped, goals: Goals,
+            planner_id: str = '', use_start: bool = False) -> Path:
         """Send a `ComputePathThroughPoses` action request."""
         self.clearTaskError()
         rtn = self._getPathThroughPosesImpl(start, goals, planner_id, use_start)
@@ -670,7 +691,8 @@ def getPathThroughPoses(self, start, goals, planner_id='', use_start=False):
             return None
 
     def _smoothPathImpl(
-        self, path, smoother_id='', max_duration=2.0, check_for_collision=False
+        self, path: Path, smoother_id: str = '',
+        max_duration: float = 2.0, check_for_collision: bool = False
     ) -> SmoothPath.Result:
         """
         Send a `SmoothPath` action request.
@@ -692,7 +714,7 @@ def _smoothPathImpl(
         rclpy.spin_until_future_complete(self, send_goal_future)
         self.goal_handle = send_goal_future.result()
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error('Smooth path was rejected!')
             result = SmoothPath.Result()
             result.error_code = SmoothPath.UNKNOWN
@@ -701,13 +723,13 @@ def _smoothPathImpl(
 
         self.result_future = self.goal_handle.get_result_async()
         rclpy.spin_until_future_complete(self, self.result_future)
-        self.status = self.result_future.result().status
+        self.status = self.result_future.result().status  # type: ignore[union-attr]
 
-        return self.result_future.result().result
+        return self.result_future.result().result  # type: ignore[union-attr]
 
     def smoothPath(
-        self, path, smoother_id='', max_duration=2.0, check_for_collision=False
-    ):
+        self, path: Path, smoother_id: str = '',
+            max_duration: float = 2.0, check_for_collision: bool = False) -> Path:
         """Send a `SmoothPath` action request."""
         self.clearTaskError()
         rtn = self._smoothPathImpl(path, smoother_id, max_duration, check_for_collision)
@@ -722,7 +744,7 @@ def smoothPath(
                       f' error msg:{rtn.error_msg}')
             return None
 
-    def changeMap(self, map_filepath) -> bool:
+    def changeMap(self, map_filepath: str) -> bool:
         """Change the current static map in the map server."""
         while not self.change_maps_srv.wait_for_service(timeout_sec=1.0):
             self.info('change map service not available, waiting...')
@@ -730,7 +752,13 @@ def changeMap(self, map_filepath) -> bool:
         req.map_url = map_filepath
         future = self.change_maps_srv.call_async(req)
         rclpy.spin_until_future_complete(self, future)
-        result = future.result().result
+
+        future_result = future.result()
+        if future_result is None:
+            self.error('Change map request failed!')
+            return False
+
+        result = future_result.result
         if result != LoadMap.Response().RESULT_SUCCESS:
             if result == LoadMap.RESULT_MAP_DOES_NOT_EXIST:
                 reason = 'Map does not exist'
@@ -749,13 +777,13 @@ def changeMap(self, map_filepath) -> bool:
             self.info('Change map request was successful!')
             return True
 
-    def clearAllCostmaps(self):
+    def clearAllCostmaps(self) -> None:
         """Clear all costmaps."""
         self.clearLocalCostmap()
         self.clearGlobalCostmap()
         return
 
-    def clearLocalCostmap(self):
+    def clearLocalCostmap(self) -> None:
         """Clear local costmap."""
         while not self.clear_costmap_local_srv.wait_for_service(timeout_sec=1.0):
             self.info('Clear local costmaps service not available, waiting...')
@@ -764,7 +792,7 @@ def clearLocalCostmap(self):
         rclpy.spin_until_future_complete(self, future)
         return
 
-    def clearGlobalCostmap(self):
+    def clearGlobalCostmap(self) -> None:
         """Clear global costmap."""
         while not self.clear_costmap_global_srv.wait_for_service(timeout_sec=1.0):
             self.info('Clear global costmaps service not available, waiting...')
@@ -773,7 +801,7 @@ def clearGlobalCostmap(self):
         rclpy.spin_until_future_complete(self, future)
         return
 
-    def clearCostmapExceptRegion(self, reset_distance: float):
+    def clearCostmapExceptRegion(self, reset_distance: float) -> None:
         """Clear the costmap except for a specified region."""
         while not self.clear_costmap_except_region_srv.wait_for_service(timeout_sec=1.0):
             self.info('ClearCostmapExceptRegion service not available, waiting...')
@@ -783,7 +811,7 @@ def clearCostmapExceptRegion(self, reset_distance: float):
         rclpy.spin_until_future_complete(self, future)
         return
 
-    def clearCostmapAroundRobot(self, reset_distance: float):
+    def clearCostmapAroundRobot(self, reset_distance: float) -> None:
         """Clear the costmap around the robot."""
         while not self.clear_costmap_around_robot_srv.wait_for_service(timeout_sec=1.0):
             self.info('ClearCostmapAroundRobot service not available, waiting...')
@@ -793,25 +821,38 @@ def clearCostmapAroundRobot(self, reset_distance: float):
         rclpy.spin_until_future_complete(self, future)
         return
 
-    def getGlobalCostmap(self):
+    def getGlobalCostmap(self) -> OccupancyGrid:
         """Get the global costmap."""
         while not self.get_costmap_global_srv.wait_for_service(timeout_sec=1.0):
             self.info('Get global costmaps service not available, waiting...')
         req = GetCostmap.Request()
         future = self.get_costmap_global_srv.call_async(req)
         rclpy.spin_until_future_complete(self, future)
-        return future.result().map
 
-    def getLocalCostmap(self):
+        result = future.result()
+        if result is None:
+            self.error('Get global costmap request failed!')
+            return None
+
+        return result.map
+
+    def getLocalCostmap(self) -> OccupancyGrid:
         """Get the local costmap."""
         while not self.get_costmap_local_srv.wait_for_service(timeout_sec=1.0):
             self.info('Get local costmaps service not available, waiting...')
         req = GetCostmap.Request()
         future = self.get_costmap_local_srv.call_async(req)
         rclpy.spin_until_future_complete(self, future)
-        return future.result().map
 
-    def lifecycleStartup(self):
+        result = future.result()
+
+        if result is None:
+            self.error('Get local costmap request failed!')
+            return None
+
+        return result.map
+
+    def lifecycleStartup(self) -> None:
         """Startup nav2 lifecycle system."""
         self.info('Starting up lifecycle nodes based on lifecycle_manager.')
         for srv_name, srv_type in self.get_service_names_and_types():
@@ -835,7 +876,7 @@ def lifecycleStartup(self):
         self.info('Nav2 is ready for use!')
         return
 
-    def lifecycleShutdown(self):
+    def lifecycleShutdown(self) -> None:
         """Shutdown nav2 lifecycle system."""
         self.info('Shutting down lifecycle nodes based on lifecycle_manager.')
         for srv_name, srv_type in self.get_service_names_and_types():
@@ -851,7 +892,7 @@ def lifecycleShutdown(self):
                 future.result()
         return
 
-    def _waitForNodeToActivate(self, node_name):
+    def _waitForNodeToActivate(self, node_name: str) -> None:
         # Waits for the node within the tester namespace to become active
         self.debug(f'Waiting for {node_name} to become active..')
         node_service = f'{node_name}/get_state'
@@ -865,13 +906,15 @@ def _waitForNodeToActivate(self, node_name):
             self.debug(f'Getting {node_name} state...')
             future = state_client.call_async(req)
             rclpy.spin_until_future_complete(self, future)
-            if future.result() is not None:
-                state = future.result().current_state.label
+
+            result = future.result()
+            if result is not None:
+                state = result.current_state.label
                 self.debug(f'Result of get_state: {state}')
             time.sleep(2)
         return
 
-    def _waitForInitialPose(self):
+    def _waitForInitialPose(self) -> None:
         while not self.initial_pose_received:
             self.info('Setting initial pose')
             self._setInitialPose()
@@ -879,17 +922,17 @@ def _waitForInitialPose(self):
             rclpy.spin_once(self, timeout_sec=1.0)
         return
 
-    def _amclPoseCallback(self, msg):
+    def _amclPoseCallback(self, msg: PoseWithCovarianceStamped) -> None:
         self.debug('Received amcl pose')
         self.initial_pose_received = True
         return
 
-    def _feedbackCallback(self, msg):
+    def _feedbackCallback(self, msg: NavigateToPose.Feedback) -> None:
         self.debug('Received action feedback message')
         self.feedback = msg.feedback
         return
 
-    def _setInitialPose(self):
+    def _setInitialPose(self) -> None:
         msg = PoseWithCovarianceStamped()
         msg.pose.pose = self.initial_pose.pose
         msg.header.frame_id = self.initial_pose.header.frame_id
@@ -898,18 +941,18 @@ def _setInitialPose(self):
         self.initial_pose_pub.publish(msg)
         return
 
-    def info(self, msg):
+    def info(self, msg: str) -> None:
         self.get_logger().info(msg)
         return
 
-    def warn(self, msg):
+    def warn(self, msg: str) -> None:
         self.get_logger().warn(msg)
         return
 
-    def error(self, msg):
+    def error(self, msg: str) -> None:
         self.get_logger().error(msg)
         return
 
-    def debug(self, msg):
+    def debug(self, msg: str) -> None:
         self.get_logger().debug(msg)
         return
diff --git a/nav2_simple_commander/nav2_simple_commander/utils.py b/nav2_simple_commander/nav2_simple_commander/utils.py
index d3639010202..8aed7434ec5 100644
--- a/nav2_simple_commander/nav2_simple_commander/utils.py
+++ b/nav2_simple_commander/nav2_simple_commander/utils.py
@@ -21,7 +21,7 @@
 import subprocess
 
 
-def find_os_processes(name):
+def find_os_processes(name: str) -> list[tuple[str, str]]:
     """Find all the processes that are running gz sim."""
     ps_output = subprocess.check_output(['ps', 'aux'], text=True)
     ps_lines = ps_output.split('\n')
@@ -36,7 +36,7 @@ def find_os_processes(name):
     return gz_sim_processes
 
 
-def kill_process(pid):
+def kill_process(pid: str) -> None:
     """Kill a process with a given PID."""
     try:
         os.kill(int(pid), signal.SIGKILL)
@@ -45,7 +45,7 @@ def kill_process(pid):
         print(f'Failed to kill process with PID: {pid}. Error: {e}')
 
 
-def kill_os_processes(name):
+def kill_os_processes(name: str) -> None:
     """Kill all processes that are running with name."""
     processes = find_os_processes(name)
     if processes:
diff --git a/nav2_simple_commander/pytest.ini b/nav2_simple_commander/pytest.ini
index 50d6d012576..fe55d2ed64b 100644
--- a/nav2_simple_commander/pytest.ini
+++ b/nav2_simple_commander/pytest.ini
@@ -1,2 +1,2 @@
 [pytest]
-junit_family=xunit2
\ No newline at end of file
+junit_family=xunit2
diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py
index 4b49f89a450..74a15c63541 100644
--- a/nav2_simple_commander/setup.py
+++ b/nav2_simple_commander/setup.py
@@ -3,7 +3,6 @@
 
 from setuptools import setup
 
-
 package_name = 'nav2_simple_commander'
 
 setup(
diff --git a/nav2_simple_commander/test/test_copyright.py b/nav2_simple_commander/test/test_copyright.py
index cc8ff03f79a..1f869834559 100644
--- a/nav2_simple_commander/test/test_copyright.py
+++ b/nav2_simple_commander/test/test_copyright.py
@@ -18,6 +18,6 @@
 
 @pytest.mark.copyright
 @pytest.mark.linter
-def test_copyright():
+def test_copyright() -> None:
     rc = main(argv=['.', 'test'])
     assert rc == 0, 'Found errors'
diff --git a/nav2_simple_commander/test/test_flake8.py b/nav2_simple_commander/test/test_flake8.py
index 26030113cce..2c660b56e6e 100644
--- a/nav2_simple_commander/test/test_flake8.py
+++ b/nav2_simple_commander/test/test_flake8.py
@@ -18,8 +18,7 @@
 
 @pytest.mark.flake8
 @pytest.mark.linter
-def test_flake8():
+def test_flake8() -> None:
     rc, errors = main_with_errors(argv=[])
-    assert rc == 0, 'Found %d code style errors / warnings:\n' % len(
-        errors
-    ) + '\n'.join(errors)
+    assert rc == 0, f'Found {len(errors)} code style errors / warnings:\n' \
+        + '\n'.join(errors)
diff --git a/nav2_simple_commander/test/test_footprint_collision_checker.py b/nav2_simple_commander/test/test_footprint_collision_checker.py
index 8ffc4b653b9..86eb32d1e5f 100644
--- a/nav2_simple_commander/test/test_footprint_collision_checker.py
+++ b/nav2_simple_commander/test/test_footprint_collision_checker.py
@@ -24,13 +24,13 @@
 
 class TestFootprintCollisionChecker(unittest.TestCase):
 
-    def test_no_costmap(self):
+    def test_no_costmap(self) -> None:
         # Test if a type error raised when costmap is not specified yet
         fcc_ = FootprintCollisionChecker()
         self.assertRaises(ValueError, fcc_.worldToMapValidated, 0.0, 0.0)
         self.assertRaises(ValueError, fcc_.pointCost, 0.0, 0.0)
 
-    def test_pointCost(self):
+    def test_pointCost(self) -> None:
         # Test if point cost is calculated correctly
         # Create test grid 10 pixels wide by 10 pixels long, at 1 meters per pixel
         # AKA 10 meters x 10 meters
@@ -48,7 +48,7 @@ def test_pointCost(self):
         self.assertEqual(fcc_.pointCost(1, 1), 0)
         self.assertRaises(IndexError, fcc_.pointCost, 11, 11)
 
-    def test_worldToMapValidated(self):
+    def test_worldToMapValidated(self) -> None:
         # Test if worldToMap conversion is calculated correctly
         # Create test grid 10 pixels wide by 10 pixels long, at 1 meters per pixel
         # AKA 10 meters x 10 meters
@@ -70,7 +70,7 @@ def test_worldToMapValidated(self):
         self.assertEqual(fcc_.worldToMapValidated(14, 14), (9, 9))
         self.assertEqual(fcc_.worldToMapValidated(15, 14), (None, None))
 
-    def test_lineCost(self):
+    def test_lineCost(self) -> None:
         # Test if line cost is calculated correctly
         # Create test grid 10 pixels wide by 10 pixels long, at 1 meters per pixel
         # AKA 10 meters x 10 meters
@@ -88,7 +88,7 @@ def test_lineCost(self):
         self.assertRaises(IndexError, fcc_.lineCost, 0, 15, 0, 9, 1)
         self.assertEqual(fcc_.lineCost(0, 9, 0, 9, 1), 0.0)
 
-    def test_footprintCost(self):
+    def test_footprintCost(self) -> None:
         # Test if footprint cost is calculated correctly
         # Create test grid 10 pixels wide by 10 pixels long, at 1 meters per pixel
         # AKA 10 meters x 10 meters
diff --git a/nav2_simple_commander/test/test_line_iterator.py b/nav2_simple_commander/test/test_line_iterator.py
index 3ec64bc5212..72eac10e18f 100644
--- a/nav2_simple_commander/test/test_line_iterator.py
+++ b/nav2_simple_commander/test/test_line_iterator.py
@@ -20,17 +20,17 @@
 
 class TestLineIterator(unittest.TestCase):
 
-    def test_type_error(self):
+    def test_type_error(self) -> None:
         # Test if a type error raised when passing invalid arguments types
         self.assertRaises(TypeError, LineIterator, 0, 0, '10', 10, '1')
 
-    def test_value_error(self):
+    def test_value_error(self) -> None:
         # Test if a value error raised when passing negative or zero step_size
         self.assertRaises(ValueError, LineIterator, 0, 0, 10, 10, -2)
         # Test if a value error raised when passing zero length line
         self.assertRaises(ValueError, LineIterator, 2, 2, 2, 2, 1)
 
-    def test_get_xy(self):
+    def test_get_xy(self) -> None:
         # Test if the initial and final coordinates are returned correctly
         lt = LineIterator(0, 0, 5, 5, 1)
         self.assertEqual(lt.getX0(), 0)
@@ -38,12 +38,12 @@ def test_get_xy(self):
         self.assertEqual(lt.getX1(), 5)
         self.assertEqual(lt.getY1(), 5)
 
-    def test_line_length(self):
+    def test_line_length(self) -> None:
         # Test if the line length is calculated correctly
         lt = LineIterator(0, 0, 5, 5, 1)
         self.assertEqual(lt.get_line_length(), sqrt(pow(5, 2) + pow(5, 2)))
 
-    def test_straight_line(self):
+    def test_straight_line(self) -> None:
         # Test if the calculations are correct for y = x
         lt = LineIterator(0, 0, 5, 5, 1)
         i = 0
@@ -71,7 +71,7 @@ def test_straight_line(self):
             lt.advance()
             i += 1
 
-    def test_hor_line(self):
+    def test_hor_line(self) -> None:
         # Test if the calculations are correct for y = 0x+b (horizontal line)
         lt = LineIterator(0, 10, 5, 10, 1)
         i = 0
@@ -81,7 +81,7 @@ def test_hor_line(self):
             lt.advance()
             i += 1
 
-    def test_ver_line(self):
+    def test_ver_line(self) -> None:
         # Test if the calculations are correct for x = n (vertical line)
         lt = LineIterator(5, 0, 5, 10, 1)
         i = 0
@@ -91,7 +91,7 @@ def test_ver_line(self):
             lt.advance()
             i += 1
 
-    def test_clamp(self):
+    def test_clamp(self) -> None:
         # Test if the increments are clamped to avoid crossing the final points
         # when step_size is large with respect to line length
         lt = LineIterator(0, 0, 5, 5, 10)
diff --git a/nav2_simple_commander/test/test_pep257.py b/nav2_simple_commander/test/test_pep257.py
index b234a3840f4..b6808e1d80f 100644
--- a/nav2_simple_commander/test/test_pep257.py
+++ b/nav2_simple_commander/test/test_pep257.py
@@ -18,6 +18,6 @@
 
 @pytest.mark.linter
 @pytest.mark.pep257
-def test_pep257():
+def test_pep257() -> None:
     rc = main(argv=['.', 'test'])
     assert rc == 0, 'Found code style errors / warnings'
diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md
index 40d68b5f4bc..98a960cb017 100644
--- a/nav2_smac_planner/README.md
+++ b/nav2_smac_planner/README.md
@@ -207,7 +207,7 @@ Seeing the figures below, you'll see an attempt to plan into a "U" shaped region
 
 By simply increasing the footprint (a bit hackier, the best solution is to edit the map to make this area impassable), then that gap is now properly blocked as un-navigable. In the second figure, you can see that the heuristics influence the expansion down a navigable route and is able to find a path in less than 10,000 iterations (or about 110ms). It is easy now!
 
-As such, it is recommended if you have sparse SLAM maps, gaps or holes in your map, that you lightly post-process them to fill those gaps or increasing your footprint's padding or radius to make these areas invalid. Without it, it might waste expansions on this small corridor that: A) you dont want your robot actually using B) probably isn't actually valid and a SLAM artifact and C) if there's a more open space, you'd rather it use that.
+As such, it is recommended if you have sparse SLAM maps, gaps or holes in your map, that you lightly post-process them to fill those gaps or increasing your footprint's padding or radius to make these areas invalid. Without it, it might waste expansions on this small corridor that: A) you don't want your robot actually using B) probably isn't actually valid and a SLAM artifact and C) if there's a more open space, you'd rather it use that.
 
 ![](media/A.png)
 ![](media/B.png)
diff --git a/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h b/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h
index 5aa4d530753..c6b0fd6009a 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h
+++ b/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h
@@ -575,7 +575,7 @@ struct nothrow {
 struct is_transparent_tag {};
 
 // A custom pair implementation is used in the map because std::pair is not is_trivially_copyable,
-// which means it would  not be allowed to be used in std::memcpy. This struct is copyable, which is
+// which means it would  not be allowed to be used in std::memcpy. This struct is copiable, which is
 // also tested.
 template <typename T1, typename T2>
 struct pair {
diff --git a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp
index c2edd13b6e2..84320aba057 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp
@@ -45,9 +45,9 @@ inline geometry_msgs::msg::Pose getWorldCoords(
 {
   geometry_msgs::msg::Pose msg;
   msg.position.x =
-    static_cast<float>(costmap->getOriginX()) + (mx - 0.5) * costmap->getResolution();
+    static_cast<float>(costmap->getOriginX()) + mx * costmap->getResolution();
   msg.position.y =
-    static_cast<float>(costmap->getOriginY()) + (my - 0.5) * costmap->getResolution();
+    static_cast<float>(costmap->getOriginY()) + my * costmap->getResolution();
   return msg;
 }
 
diff --git a/nav2_smac_planner/lattice_primitives/README.md b/nav2_smac_planner/lattice_primitives/README.md
index cfdebe36aa4..57e134b1797 100644
--- a/nav2_smac_planner/lattice_primitives/README.md
+++ b/nav2_smac_planner/lattice_primitives/README.md
@@ -37,7 +37,7 @@ The directory to save the visualizations can be specified by passing in a path w
 Note: None of these parameters have defaults. They all must be specified through the [config.json](config.json) file.
 
 **motion_model** (string)
-    
+
 The type of motion model used. Accepted values:
 - `ackermann`: Only forward and reversing trajectories
 - `diff`: Forward moving trajectories + rotation in place by a single angular bin
@@ -119,11 +119,11 @@ A list of dictionaries where each dictionary represents an individual motion pri
 This section describes how the various portions of the generation algorithm works.
 
 ### Angle Discretization
-Dividing a full turn into uniform angular headings presents several problems. The biggest problem is that it will create angles for which a straight trajectory does not land nicely on an endpoint that aligns with the grid. Instead we discretize the angles in a way that ensures straight paths will land on endpoints aligned with the grid. 
+Dividing a full turn into uniform angular headings presents several problems. The biggest problem is that it will create angles for which a straight trajectory does not land nicely on an endpoint that aligns with the grid. Instead we discretize the angles in a way that ensures straight paths will land on endpoints aligned with the grid.
 
 ![ ](docs/angle_discretization.png)
 
-The image shows how the angular headings are generated. The same idea can be extended to a higher number of headings. As a result, the number of headings parameter is restricted to multiples of 8.  
+The image shows how the angular headings are generated. The same idea can be extended to a higher number of headings. As a result, the number of headings parameter is restricted to multiples of 8.
 
 ### Trajectory Generator
 1. Create two lines. Line 1 passes through start point with angle of start angle, and line 2 passes through the end point with angle of end angle
@@ -165,4 +165,4 @@ The lattice generator is generally based on the generation of the control set as
 
 6. Steps 1-5 are repeated for all possible start angles between 0 and 90.
 
-7. The resulting control set will only contain trajectories in quadrant 1. To get the final control set we exploit symmetry across the axess and flip the trajectories in different ways.
\ No newline at end of file
+7. The resulting control set will only contain trajectories in quadrant 1. To get the final control set we exploit symmetry across the axess and flip the trajectories in different ways.
diff --git a/nav2_smac_planner/lattice_primitives/__init__.py b/nav2_smac_planner/lattice_primitives/__init__.py
new file mode 100644
index 00000000000..e69de29bb2d
diff --git a/nav2_smac_planner/lattice_primitives/config.json b/nav2_smac_planner/lattice_primitives/config.json
index 27671c337d1..12a8470bdb3 100644
--- a/nav2_smac_planner/lattice_primitives/config.json
+++ b/nav2_smac_planner/lattice_primitives/config.json
@@ -4,4 +4,4 @@
     "grid_resolution": 0.05,
     "stopping_threshold": 5,
     "num_of_headings": 16
-}
\ No newline at end of file
+}
diff --git a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py
index 9ec50287193..bf9e64559ab 100644
--- a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py
+++ b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py
@@ -18,19 +18,26 @@
 import logging
 from pathlib import Path
 import time
-
-import constants
-from lattice_generator import LatticeGenerator
+from typing import Any, cast, Dict, List, TypedDict
 
 import matplotlib.pyplot as plt
+from nav2_smac_planner.lattice_primitives import constants
+from nav2_smac_planner.lattice_primitives.lattice_generator import ConfigDict, LatticeGenerator
+from nav2_smac_planner.lattice_primitives.trajectory import Trajectory
 import numpy as np
 
-
 logging.basicConfig(level=logging.INFO)
 logger = logging.getLogger(__name__)
 
 
-def handle_arg_parsing():
+class HeaderDict(TypedDict):
+    version: float
+    date_generated: str
+    lattice_metadata: Dict[str, Any]
+    primitives: List[Dict[str, Any]]
+
+
+def handle_arg_parsing() -> argparse.Namespace:
     """
     Handle the parsing of arguments.
 
@@ -66,7 +73,8 @@ def handle_arg_parsing():
     return parser.parse_args()
 
 
-def create_heading_angle_list(minimal_set_trajectories: dict) -> list:
+def create_heading_angle_list(minimal_set_trajectories: Dict[float, List[Trajectory]]
+                              ) -> List[float]:
     """
     Create a sorted list of heading angles from the minimal trajectory set.
 
@@ -85,7 +93,7 @@ def create_heading_angle_list(minimal_set_trajectories: dict) -> list:
     return sorted(heading_angles, key=lambda x: (x < 0, x))
 
 
-def read_config(config_path) -> dict:
+def read_config(config_path: Path) -> ConfigDict:
     """
     Read in the user defined parameters via JSON.
 
@@ -103,10 +111,11 @@ def read_config(config_path) -> dict:
     with open(config_path) as config_file:
         config = json.load(config_file)
 
-    return config
+    return cast(ConfigDict, config)
 
 
-def create_header(config: dict, minimal_set_trajectories: dict) -> dict:
+def create_header(config: ConfigDict, minimal_set_trajectories: Dict[float, List[Trajectory]]
+                  ) -> HeaderDict:
     """
     Create a dict containing all the fields to populate the header with.
 
@@ -123,7 +132,7 @@ def create_header(config: dict, minimal_set_trajectories: dict) -> dict:
         A dictionary containing the fields to populate the header with
 
     """
-    header_dict = {
+    header_dict: HeaderDict = {
         'version': constants.VERSION,
         'date_generated': datetime.today().strftime('%Y-%m-%d'),
         'lattice_metadata': {},
@@ -144,7 +153,7 @@ def create_header(config: dict, minimal_set_trajectories: dict) -> dict:
 
 
 def write_to_json(
-    output_path: Path, minimal_set_trajectories: dict, config: dict
+    output_path: Path, minimal_set_trajectories: Dict[float, List[Trajectory]], config: ConfigDict
 ) -> None:
     """
     Write the minimal spanning set to an output file.
@@ -173,7 +182,7 @@ def write_to_json(
             minimal_set_trajectories[start_angle], key=lambda x: x.parameters.end_angle
         ):
 
-            traj_info = {}
+            traj_info: Dict[str, Any] = {}
             traj_info['trajectory_id'] = idx
             traj_info['start_angle_index'] = heading_lookup[
                 trajectory.parameters.start_angle
@@ -204,7 +213,7 @@ def write_to_json(
 
 
 def save_visualizations(
-    visualizations_folder: Path, minimal_set_trajectories: dict
+    visualizations_folder: Path, minimal_set_trajectories: Dict[float, List[Trajectory]]
 ) -> None:
     """
     Draw the visualizations for every trajectory and save it as an image.
diff --git a/nav2_smac_planner/lattice_primitives/helper.py b/nav2_smac_planner/lattice_primitives/helper.py
index e29c7877389..8d6265c8a97 100644
--- a/nav2_smac_planner/lattice_primitives/helper.py
+++ b/nav2_smac_planner/lattice_primitives/helper.py
@@ -12,10 +12,13 @@
 # See the License for the specific language governing permissions and
 # limitations under the License. Reserved.
 
+from typing import Any, Optional
+
 import numpy as np
+from numpy.typing import NDArray
 
 
-def normalize_angle(angle):
+def normalize_angle(angle: float) -> float:
     """
     Normalize the angle to between [0, 2pi).
 
@@ -37,7 +40,8 @@ def normalize_angle(angle):
     return angle
 
 
-def angle_difference(angle_1, angle_2, left_turn=None):
+def angle_difference(angle_1: float, angle_2: float,
+                     left_turn: Optional[float] = None) -> float:
     """
     Calculate the difference between two angles based on a given direction.
 
@@ -76,7 +80,8 @@ def angle_difference(angle_1, angle_2, left_turn=None):
             return 2 * np.pi - abs(angle_1 - angle_2)
 
 
-def interpolate_yaws(start_angle, end_angle, left_turn, steps):
+def interpolate_yaws(start_angle: float, end_angle: float,
+                     left_turn: bool, steps: int) -> Any:
     """
     Create equally spaced yaws between two angles.
 
@@ -110,7 +115,7 @@ def interpolate_yaws(start_angle, end_angle, left_turn, steps):
     return yaws
 
 
-def get_rotation_matrix(angle):
+def get_rotation_matrix(angle: float) -> NDArray[np.floating[Any]]:
     """
     Return a rotation matrix that is equivalent to a 2D rotation of angle.
 
diff --git a/nav2_smac_planner/lattice_primitives/lattice_generator.py b/nav2_smac_planner/lattice_primitives/lattice_generator.py
index a2b0340cb9b..7aacd36e156 100644
--- a/nav2_smac_planner/lattice_primitives/lattice_generator.py
+++ b/nav2_smac_planner/lattice_primitives/lattice_generator.py
@@ -14,16 +14,22 @@
 
 from collections import defaultdict
 from enum import Enum
+from typing import Any, Dict, List, Tuple, TypedDict
 
-from helper import angle_difference, interpolate_yaws
-
+from nav2_smac_planner.lattice_primitives.helper import angle_difference, interpolate_yaws
+from nav2_smac_planner.lattice_primitives.trajectory import (AnyFloat, FloatNDArray, Path,
+                                                             Trajectory, TrajectoryParameters)
+from nav2_smac_planner.lattice_primitives.trajectory_generator import TrajectoryGenerator
 import numpy as np
-
 from rtree import index
 
-from trajectory import Path, Trajectory, TrajectoryParameters
 
-from trajectory_generator import TrajectoryGenerator
+class ConfigDict(TypedDict):
+    grid_resolution: float
+    turning_radius: float
+    stopping_threshold: int
+    num_of_headings: int
+    motion_model: str
 
 
 class LatticeGenerator:
@@ -50,7 +56,7 @@ class Flip(Enum):
         Y = 2
         BOTH = 3
 
-    def __init__(self, config: dict):
+    def __init__(self, config: ConfigDict):
         """Init the lattice generator from the user supplied config."""
         self.trajectory_generator = TrajectoryGenerator(config)
         self.grid_resolution = config['grid_resolution']
@@ -64,7 +70,7 @@ def __init__(self, config: dict):
         self.DISTANCE_THRESHOLD = 0.5 * self.grid_resolution
         self.ROTATION_THRESHOLD = 0.5 * (2 * np.pi / self.num_of_headings)
 
-    def _get_wave_front_points(self, pos: int) -> np.array:
+    def _get_wave_front_points(self, pos: int) -> FloatNDArray:
         """
         Calculate the end points that lie on the wave front.
 
@@ -101,7 +107,7 @@ def _get_wave_front_points(self, pos: int) -> np.array:
 
         return np.array(positions)
 
-    def _get_heading_discretization(self, number_of_headings: int) -> list:
+    def _get_heading_discretization(self, number_of_headings: int) -> List[int]:
         """
         Calculate the heading discretization based on the number of headings.
 
@@ -135,7 +141,8 @@ def _get_heading_discretization(self, number_of_headings: int) -> list:
 
         return sorted([np.arctan2(j, i) for i, j in zip(outer_edge_x, outer_edge_y)])
 
-    def _point_to_line_distance(self, p1: np.array, p2: np.array, q: np.array) -> float:
+    def _point_to_line_distance(self, p1: FloatNDArray, p2: FloatNDArray,
+                                q: FloatNDArray) -> AnyFloat:
         """
         Return the shortest distance from a point to a line segment.
 
@@ -245,7 +252,7 @@ def _compute_min_trajectory_length(self) -> float:
 
         return self.turning_radius * min(heading_diff)
 
-    def _generate_minimal_spanning_set(self) -> dict:
+    def _generate_minimal_spanning_set(self) -> Dict[float, List[Trajectory]]:
         """
         Generate the minimal spanning set.
 
@@ -259,7 +266,7 @@ def _generate_minimal_spanning_set(self) -> dict:
             a list of trajectories that begin at that angle
 
         """
-        quadrant1_end_poses = defaultdict(list)
+        quadrant1_end_poses: Dict[int, List[Tuple[Any, int]]] = defaultdict(list)
 
         # Since we only compute for quadrant 1 we only need headings between
         # 0 and 90 degrees
@@ -342,7 +349,7 @@ def _generate_minimal_spanning_set(self) -> dict:
         # we can leverage symmetry to create the complete minimal set
         return self._create_complete_minimal_spanning_set(quadrant1_end_poses)
 
-    def _flip_angle(self, angle: float, flip_type: Flip) -> float:
+    def _flip_angle(self, angle: int, flip_type: Flip) -> float:
         """
         Return the the appropriate flip of the angle in self.headings.
 
@@ -374,8 +381,8 @@ def _flip_angle(self, angle: float, flip_type: Flip) -> float:
         return self.headings[int(heading_idx)]
 
     def _create_complete_minimal_spanning_set(
-        self, single_quadrant_minimal_set: dict
-    ) -> dict:
+        self, single_quadrant_minimal_set: Dict[int, List[Tuple[Any, int]]]
+    ) -> Dict[float, List[Trajectory]]:
         """
         Create the full minimal spanning set from a single quadrant set.
 
@@ -394,7 +401,7 @@ def _create_complete_minimal_spanning_set(
             in all quadrants
 
         """
-        all_trajectories = defaultdict(list)
+        all_trajectories: Dict[float, List[Trajectory]] = defaultdict(list)
 
         for start_angle in single_quadrant_minimal_set.keys():
 
@@ -429,6 +436,9 @@ def _create_complete_minimal_spanning_set(
                         )
                     )
 
+                    if unflipped_trajectory is None or flipped_x_trajectory is None:
+                        raise ValueError('No trajectory was found')
+
                     all_trajectories[
                         unflipped_trajectory.parameters.start_angle
                     ].append(unflipped_trajectory)
@@ -463,6 +473,9 @@ def _create_complete_minimal_spanning_set(
                         )
                     )
 
+                    if unflipped_trajectory is None or flipped_y_trajectory is None:
+                        raise ValueError('No trajectory was found')
+
                     all_trajectories[
                         unflipped_trajectory.parameters.start_angle
                     ].append(unflipped_trajectory)
@@ -517,6 +530,10 @@ def _create_complete_minimal_spanning_set(
                         )
                     )
 
+                    if (unflipped_trajectory is None or flipped_y_trajectory is None or
+                       flipped_x_trajectory is None or flipped_xy_trajectory is None):
+                        raise ValueError('No trajectory was found')
+
                     all_trajectories[
                         unflipped_trajectory.parameters.start_angle
                     ].append(unflipped_trajectory)
@@ -532,7 +549,8 @@ def _create_complete_minimal_spanning_set(
 
         return all_trajectories
 
-    def _handle_motion_model(self, spanning_set: dict) -> dict:
+    def _handle_motion_model(self, spanning_set: Dict[float, List[Trajectory]]
+                             ) -> Dict[float, List[Trajectory]]:
         """
         Add the appropriate motions for the user supplied motion model.
 
@@ -569,7 +587,8 @@ def _handle_motion_model(self, spanning_set: dict) -> dict:
             print('No handling implemented for Motion Model: ' + f'{self.motion_model}')
             raise NotImplementedError
 
-    def _add_in_place_turns(self, spanning_set: dict) -> dict:
+    def _add_in_place_turns(self, spanning_set: Dict[float, List[Trajectory]]
+                            ) -> Dict[float, List[Trajectory]]:
         """
         Add in place turns to the spanning set.
 
@@ -627,7 +646,8 @@ def _add_in_place_turns(self, spanning_set: dict) -> dict:
 
         return spanning_set
 
-    def _add_horizontal_motions(self, spanning_set: dict) -> dict:
+    def _add_horizontal_motions(self, spanning_set: Dict[float, List[Trajectory]]
+                                ) -> Dict[float, List[Trajectory]]:
         """
         Add horizontal sliding motions to the spanning set.
 
@@ -727,7 +747,7 @@ def _add_horizontal_motions(self, spanning_set: dict) -> dict:
 
         return spanning_set
 
-    def run(self):
+    def run(self) -> Dict[float, List[Trajectory]]:
         """
         Run the lattice generator.
 
diff --git a/nav2_smac_planner/lattice_primitives/py.typed b/nav2_smac_planner/lattice_primitives/py.typed
new file mode 100644
index 00000000000..e69de29bb2d
diff --git a/nav2_smac_planner/lattice_primitives/requirements.txt b/nav2_smac_planner/lattice_primitives/requirements.txt
index c76f9d4ba62..7ae56b050f2 100644
--- a/nav2_smac_planner/lattice_primitives/requirements.txt
+++ b/nav2_smac_planner/lattice_primitives/requirements.txt
@@ -1,3 +1,3 @@
 numpy>=1.17.4
 matplotlib>=3.1.2
-Rtree>=0.9.7
\ No newline at end of file
+Rtree>=0.9.7
diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json
index 0df406dfdc1..57e1e6ae423 100644
--- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json
+++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json
@@ -3929,4 +3929,4 @@
 			]
 		}
 	]
-}
\ No newline at end of file
+}
diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output.json
index 984bbb62d9e..e1c5ee5ec05 100644
--- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output.json
+++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output.json
@@ -4873,4 +4873,4 @@
 			]
 		}
 	]
-}
\ No newline at end of file
+}
diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/omni/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/omni/output.json
index 8ea504f6c38..19a62926fca 100644
--- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/omni/output.json
+++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/omni/output.json
@@ -5857,4 +5857,4 @@
 			]
 		}
 	]
-}
\ No newline at end of file
+}
diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/ackermann/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/ackermann/output.json
index 646513c0aa0..3cf19c742fc 100644
--- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/ackermann/output.json
+++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/ackermann/output.json
@@ -3997,4 +3997,4 @@
 			]
 		}
 	]
-}
\ No newline at end of file
+}
diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/diff/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/diff/output.json
index 9146355201c..a22f58074dc 100644
--- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/diff/output.json
+++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/diff/output.json
@@ -4941,4 +4941,4 @@
 			]
 		}
 	]
-}
\ No newline at end of file
+}
diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/omni/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/omni/output.json
index 95f8dc2d85f..95a4b11bdd6 100644
--- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/omni/output.json
+++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/omni/output.json
@@ -6445,4 +6445,4 @@
 			]
 		}
 	]
-}
\ No newline at end of file
+}
diff --git a/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py b/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py
index 30a7a24eb2a..710f574480d 100644
--- a/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py
+++ b/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py
@@ -14,7 +14,7 @@
 
 import unittest
 
-from lattice_generator import LatticeGenerator
+from nav2_smac_planner.lattice_primitives.lattice_generator import ConfigDict, LatticeGenerator
 import numpy as np
 
 MOTION_MODEL = 'ackermann'
@@ -28,7 +28,7 @@ class TestLatticeGenerator(unittest.TestCase):
     """Contains the unit tests for the TrajectoryGenerator."""
 
     def setUp(self) -> None:
-        config = {
+        config: ConfigDict = {
             'motion_model': MOTION_MODEL,
             'turning_radius': TURNING_RADIUS,
             'grid_resolution': GRID_RESOLUTION,
@@ -40,7 +40,7 @@ def setUp(self) -> None:
 
         self.minimal_set = lattice_gen.run()
 
-    def test_minimal_set_lengths_are_positive(self):
+    def test_minimal_set_lengths_are_positive(self) -> None:
         # Test that lengths are all positive
 
         for start_angle in self.minimal_set.keys():
@@ -51,7 +51,7 @@ def test_minimal_set_lengths_are_positive(self):
                 self.assertGreaterEqual(trajectory.parameters.end_straight_length, 0)
                 self.assertGreaterEqual(trajectory.parameters.total_length, 0)
 
-    def test_minimal_set_end_points_lie_on_grid(self):
+    def test_minimal_set_end_points_lie_on_grid(self) -> None:
         # Test that end points lie on the grid resolution
 
         for start_angle in self.minimal_set.keys():
@@ -66,7 +66,7 @@ def test_minimal_set_end_points_lie_on_grid(self):
                 self.assertAlmostEqual(div_x, np.round(div_x), delta=0.00001)
                 self.assertAlmostEqual(div_y, np.round(div_y), delta=0.00001)
 
-    def test_minimal_set_end_angle_is_correct(self):
+    def test_minimal_set_end_angle_is_correct(self) -> None:
         # Test that end angle agrees with the end angle parameter
 
         for start_angle in self.minimal_set.keys():
@@ -76,7 +76,7 @@ def test_minimal_set_end_angle_is_correct(self):
 
                 self.assertEqual(end_point_angle, trajectory.parameters.end_angle)
 
-    def test_output_angles_in_correct_range(self):
+    def test_output_angles_in_correct_range(self) -> None:
         # Test that the outputted angles always lie within 0 to 2*pi
 
         for start_angle in self.minimal_set.keys():
diff --git a/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py b/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py
index ea3f51930fa..9a02d610b1e 100644
--- a/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py
+++ b/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py
@@ -12,10 +12,13 @@
 # See the License for the specific language governing permissions and
 # limitations under the License. Reserved.
 
+from typing import cast
 import unittest
 
+from nav2_smac_planner.lattice_primitives.trajectory import Trajectory
+from nav2_smac_planner.lattice_primitives.trajectory_generator import (
+    TrajectoryGenerator, TrajectoryGeneratorConfigDict)
 import numpy as np
-from trajectory_generator import TrajectoryGenerator
 
 TURNING_RADIUS = 1
 STEP_DISTANCE = 0.1
@@ -25,16 +28,19 @@ class TestTrajectoryGenerator(unittest.TestCase):
     """Contains the unit tests for the TrajectoryGenerator."""
 
     def setUp(self) -> None:
-        config = {'turning_radius': TURNING_RADIUS}
+        config: TrajectoryGeneratorConfigDict = {'turning_radius': TURNING_RADIUS}
         self.trajectory_generator = TrajectoryGenerator(config)
 
-    def test_generate_trajectory_only_arc(self):
+    def test_generate_trajectory_only_arc(self) -> None:
         # Quadrant 1
         end_point = np.array([1, 1])
         trajectory = self.trajectory_generator.generate_trajectory(
             end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
@@ -44,6 +50,9 @@ def test_generate_trajectory_only_arc(self):
             end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
@@ -53,6 +62,9 @@ def test_generate_trajectory_only_arc(self):
             end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
@@ -62,16 +74,22 @@ def test_generate_trajectory_only_arc(self):
             end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
-    def test_generate_trajectory_only_line(self):
+    def test_generate_trajectory_only_line(self) -> None:
         # Quadrant 1
         end_point = np.array([1, 1])
         trajectory = self.trajectory_generator.generate_trajectory(
             end_point, np.deg2rad(45), np.deg2rad(45), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
@@ -81,6 +99,9 @@ def test_generate_trajectory_only_line(self):
             end_point, np.deg2rad(135), np.deg2rad(135), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
@@ -90,6 +111,9 @@ def test_generate_trajectory_only_line(self):
             end_point, -np.deg2rad(135), -np.deg2rad(135), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
@@ -99,16 +123,22 @@ def test_generate_trajectory_only_line(self):
             end_point, -np.deg2rad(45), -np.deg2rad(45), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
-    def test_generate_trajectory_line_to_arc(self):
+    def test_generate_trajectory_line_to_arc(self) -> None:
         # Quadrant 1
         end_point = np.array([2, 1])
         trajectory = self.trajectory_generator.generate_trajectory(
             end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
@@ -118,6 +148,9 @@ def test_generate_trajectory_line_to_arc(self):
             end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
@@ -127,6 +160,9 @@ def test_generate_trajectory_line_to_arc(self):
             end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
@@ -136,16 +172,22 @@ def test_generate_trajectory_line_to_arc(self):
             end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
-    def test_generate_trajectory_line_to_end(self):
+    def test_generate_trajectory_line_to_end(self) -> None:
         # Quadrant 1
         end_point = np.array([1, 2])
         trajectory = self.trajectory_generator.generate_trajectory(
             end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
@@ -155,6 +197,9 @@ def test_generate_trajectory_line_to_end(self):
             end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
@@ -164,6 +209,9 @@ def test_generate_trajectory_line_to_end(self):
             end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
@@ -173,10 +221,13 @@ def test_generate_trajectory_line_to_end(self):
             end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
-    def test_generate_trajectory_radius_too_small(self):
+    def test_generate_trajectory_radius_too_small(self) -> None:
         # Quadrant 1
         end_point = np.array([0.9, 0.9])
         trajectory = self.trajectory_generator.generate_trajectory(
@@ -209,13 +260,16 @@ def test_generate_trajectory_radius_too_small(self):
 
         self.assertEqual(trajectory, None)
 
-    def test_generate_trajectory_parallel_lines_coincident(self):
+    def test_generate_trajectory_parallel_lines_coincident(self) -> None:
         # Quadrant 1
         end_point = np.array([5, 0])
         trajectory = self.trajectory_generator.generate_trajectory(
             end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
@@ -225,10 +279,13 @@ def test_generate_trajectory_parallel_lines_coincident(self):
             end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE
         )
 
+        self.assertIsNotNone(trajectory)
+        trajectory = cast(Trajectory, trajectory)
+
         self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
         self.assertGreater(len(trajectory.path.xs), 0)
 
-    def test_generate_trajectory_parallel_lines_not_coincident(self):
+    def test_generate_trajectory_parallel_lines_not_coincident(self) -> None:
         # Quadrant 1
         end_point = np.array([0, 3])
         trajectory = self.trajectory_generator.generate_trajectory(
diff --git a/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py b/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py
index fb6fa2c7b42..be4ad5cbea9 100644
--- a/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py
+++ b/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py
@@ -24,13 +24,14 @@
 
 import json
 from pathlib import Path
+from typing import Any
 
 import matplotlib.pyplot as plt
-
 import numpy as np
 
 
-def plot_arrow(x, y, yaw, length=1.0, fc='r', ec='k'):
+def plot_arrow(x: float, y: float, yaw: float, length: float = 1.0, fc: str = 'r', ec: str = 'k'
+               ) -> None:
     """Plot arrow."""
     plt.arrow(
         x,
@@ -44,7 +45,7 @@ def plot_arrow(x, y, yaw, length=1.0, fc='r', ec='k'):
     plt.plot(0, 0)
 
 
-def read_trajectories_data(file_path):
+def read_trajectories_data(file_path: Path) -> Any:
 
     with open(file_path) as data_file:
         trajectory_data = json.load(data_file)
diff --git a/nav2_smac_planner/lattice_primitives/trajectory.py b/nav2_smac_planner/lattice_primitives/trajectory.py
index 0dcb6edc07e..0ba9141411c 100644
--- a/nav2_smac_planner/lattice_primitives/trajectory.py
+++ b/nav2_smac_planner/lattice_primitives/trajectory.py
@@ -13,10 +13,15 @@
 # limitations under the License. Reserved.
 
 from dataclasses import dataclass
+from typing import Any, Union
 
-from helper import angle_difference, normalize_angle
-
+from nav2_smac_planner.lattice_primitives.helper import angle_difference, normalize_angle
 import numpy as np
+from numpy.typing import NDArray
+
+AnyFloat = np.floating[Any]
+TrajectoryFloat = Union[float, AnyFloat]
+FloatNDArray = NDArray[AnyFloat]
 
 
 @dataclass(frozen=True)
@@ -42,41 +47,43 @@ class TrajectoryParameters:
     arc_end_point: Coordinates of the ending position of the arc
     """
 
-    turning_radius: float
+    turning_radius: TrajectoryFloat
     x_offset: float
     y_offset: float
-    end_point: np.array
+    end_point: FloatNDArray
     start_angle: float
     end_angle: float
     left_turn: bool
 
-    arc_start_point: float
-    arc_end_point: float
+    arc_start_point: FloatNDArray
+    arc_end_point: FloatNDArray
 
     @property
-    def arc_length(self):
+    def arc_length(self) -> TrajectoryFloat:
         """Arc length of the trajectory."""
-        return self.turning_radius * angle_difference(
+        result: TrajectoryFloat = self.turning_radius * angle_difference(
             self.start_angle, self.end_angle, self.left_turn
         )
+        return result
 
     @property
-    def start_straight_length(self):
-        """Length of the straight line from start to arc."""
+    def start_straight_length(self) -> AnyFloat:
+        """Length of the straight line fromnorarc_start_pointm start to arc."""
         return np.linalg.norm(self.arc_start_point)
 
     @property
-    def end_straight_length(self):
+    def end_straight_length(self) -> AnyFloat:
         """Length of the straight line from arc to end."""
         return np.linalg.norm(self.end_point - self.arc_end_point)
 
     @property
-    def total_length(self):
+    def total_length(self) -> AnyFloat:
         """Total length of trajectory."""
         return self.arc_length + self.start_straight_length + self.end_straight_length
 
     @staticmethod
-    def no_arc(end_point, start_angle, end_angle):
+    def no_arc(end_point: FloatNDArray, start_angle: float,
+               end_angle: float) -> 'TrajectoryParameters':
         """Create the parameters for a trajectory with no arc."""
         return TrajectoryParameters(
             turning_radius=0.0,
@@ -101,11 +108,11 @@ class Path:
     yaws: Yaws of poses along trajectory
     """
 
-    xs: np.array
-    ys: np.array
-    yaws: np.array
+    xs: FloatNDArray
+    ys: FloatNDArray
+    yaws: FloatNDArray
 
-    def __add__(self, rhs):
+    def __add__(self, rhs: 'Path') -> 'Path':
         """Add two paths together by concatenating them."""
         if self.xs is None:
             return rhs
@@ -116,7 +123,7 @@ def __add__(self, rhs):
 
         return Path(xs, ys, yaws)
 
-    def to_output_format(self):
+    def to_output_format(self) -> Any:
         """Return the path data in a format suitable for outputting."""
         output_xs = self.xs.round(5)
         output_ys = self.ys.round(5)
diff --git a/nav2_smac_planner/lattice_primitives/trajectory_generator.py b/nav2_smac_planner/lattice_primitives/trajectory_generator.py
index 5fa7d638962..f576b14c188 100644
--- a/nav2_smac_planner/lattice_primitives/trajectory_generator.py
+++ b/nav2_smac_planner/lattice_primitives/trajectory_generator.py
@@ -13,25 +13,29 @@
 # limitations under the License. Reserved.
 
 import logging
-from typing import Tuple, Union
+from typing import Any, List, Tuple, TypedDict, Union
 
+from nav2_smac_planner.lattice_primitives.trajectory import (FloatNDArray, Path, Trajectory,
+                                                             TrajectoryFloat, TrajectoryParameters)
 import numpy as np
 
-from trajectory import Path, Trajectory, TrajectoryParameters
-
 logger = logging.getLogger(__name__)
 
 
+class TrajectoryGeneratorConfigDict(TypedDict):
+    turning_radius: float
+
+
 class TrajectoryGenerator:
     """Handles all the logic for generating trajectories."""
 
-    def __init__(self, config: dict):
+    def __init__(self, config: TrajectoryGeneratorConfigDict):
         """Init TrajectoryGenerator using the user supplied config."""
         self.turning_radius = config['turning_radius']
 
     def _get_arc_point(
-        self, trajectory_params: TrajectoryParameters, t: float
-    ) -> Tuple[float, float, float]:
+        self, trajectory_params: TrajectoryParameters, t: TrajectoryFloat
+    ) -> Tuple[float, float, TrajectoryFloat]:
         """
         Get point on the arc trajectory using the following parameterization.
 
@@ -101,8 +105,8 @@ def _get_arc_point(
         return x, y, yaw
 
     def _get_line_point(
-        self, start_point: np.array, end_point: np.array, t: float
-    ) -> Tuple[float, float]:
+        self, start_point: FloatNDArray, end_point: FloatNDArray, t: float
+    ) -> FloatNDArray:
         """
         Get point on a line segment using the following parameterization.
 
@@ -170,7 +174,7 @@ def _create_path(
 
         xs = []
         ys = []
-        yaws = []
+        yaws: List[TrajectoryFloat] = []
 
         for i in range(1, number_of_steps + 1):
 
@@ -183,7 +187,7 @@ def _create_path(
                 x, y = self._get_line_point(
                     np.array([0, 0]), trajectory_params.arc_start_point, line_t
                 )
-                yaw = trajectory_params.start_angle
+                yaw: TrajectoryFloat = trajectory_params.start_angle
 
             # Handle the arc
             elif cur_t <= transition_points[1]:
@@ -207,20 +211,20 @@ def _create_path(
             cur_t += t_step
 
         # Convert to numpy arrays
-        xs = np.array(xs)
-        ys = np.array(ys)
-        yaws = np.array(yaws)
+        xs_np = np.array(xs)
+        ys_np = np.array(ys)
+        yaws_np = np.array(yaws)
 
         # The last point may be slightly off due to rounding issues
         # so we correct the last point to be exactly the end point
-        xs[-1], ys[-1] = trajectory_params.end_point
-        yaws[-1] = trajectory_params.end_angle
+        xs_np[-1], ys_np[-1] = trajectory_params.end_point
+        yaws_np[-1] = trajectory_params.end_angle
 
-        return Path(xs, ys, yaws)
+        return Path(xs_np, ys_np, yaws_np)
 
     def _get_intersection_point(
         self, m1: float, c1: float, m2: float, c2: float
-    ) -> np.array:
+    ) -> FloatNDArray:
         """
         Get the intersection point of two lines.
 
@@ -244,14 +248,14 @@ def _get_intersection_point(
 
         """
 
-        def line1(x):
+        def line1(x: float) -> float:
             return m1 * x + c1
 
         x_point = (c2 - c1) / (m1 - m2)
 
         return np.array([x_point, line1(x_point)])
 
-    def _is_left_turn(self, intersection_point: np.array, end_point: np.array) -> bool:
+    def _is_left_turn(self, intersection_point: FloatNDArray, end_point: FloatNDArray) -> Any:
         """
         Determine if a trajectory will be a left turn.
 
@@ -278,7 +282,7 @@ def _is_left_turn(self, intersection_point: np.array, end_point: np.array) -> bo
         return det >= 0
 
     def _is_dir_vec_correct(
-        self, point1: np.array, point2: np.array, line_angle: float
+        self, point1: FloatNDArray, point2: FloatNDArray, line_angle: float
     ) -> bool:
         """
         Check that the direction vector agrees with the line angle.
@@ -328,7 +332,7 @@ def _is_dir_vec_correct(
             return False
 
     def _calculate_trajectory_params(
-        self, end_point: np.array, start_angle: float, end_angle: float
+        self, end_point: FloatNDArray, start_angle: float, end_angle: float
     ) -> Union[TrajectoryParameters, None]:
         """
         Calculate the parameters for a trajectory with the desired constraints.
@@ -479,13 +483,13 @@ def _calculate_trajectory_params(
         if m1 == 0:
             # If line 1 has gradient 0 then it is the x-axis.
 
-            def perp_line2(x):
+            def perp_line2(x: float) -> Any:
                 return -1 / m2 * (x - x2) + y2
 
             circle_center = np.array([x1, perp_line2(x1)])
         elif m2 == 0:
 
-            def perp_line1(x):
+            def perp_line1(x: float) -> Any:
                 return -1 / m1 * (x - x1) + y1
 
             circle_center = np.array([x2, perp_line1(x2)])
@@ -526,7 +530,7 @@ def perp_line1(x):
 
     def generate_trajectory(
         self,
-        end_point: np.array,
+        end_point: FloatNDArray,
         start_angle: float,
         end_angle: float,
         primitive_resolution: float,
diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp
index 98910084d64..de77b341cab 100644
--- a/nav2_smac_planner/src/collision_checker.cpp
+++ b/nav2_smac_planner/src/collision_checker.cpp
@@ -116,7 +116,7 @@ bool GridCollisionChecker::inCollision(
 
   // Assumes setFootprint already set
   center_cost_ = static_cast<float>(costmap_->getCost(
-    static_cast<unsigned int>(x + 0.5f), static_cast<unsigned int>(y + 0.5f)));
+      static_cast<unsigned int>(x + 0.5f), static_cast<unsigned int>(y + 0.5f)));
 
   if (!footprint_is_radius_) {
     // if footprint, then we check for the footprint's points, but first see
diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp
index 99734849a0a..3936fc7157d 100644
--- a/nav2_smac_planner/src/smac_planner_2d.cpp
+++ b/nav2_smac_planner/src/smac_planner_2d.cpp
@@ -93,6 +93,10 @@ void SmacPlanner2D::configure(
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".max_planning_time", rclcpp::ParameterValue(2.0));
   node->get_parameter(name + ".max_planning_time", _max_planning_time);
+  // Note that we need to declare it here to prevent the parameter from being declared in the
+  // dynamic reconfigure callback
+  nav2_util::declare_parameter_if_not_declared(
+    node, "service_introspection_mode", rclcpp::ParameterValue("disabled"));
 
   _motion_model = MotionModel::TWOD;
 
diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
index fcce89cdfea..6a4205a889f 100644
--- a/nav2_smac_planner/src/smac_planner_hybrid.cpp
+++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -166,6 +166,11 @@ void SmacPlannerHybrid::configure(
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN")));
   node->get_parameter(name + ".motion_model_for_search", _motion_model_for_search);
+  // Note that we need to declare it here to prevent the parameter from being declared in the
+  // dynamic reconfigure callback
+  nav2_util::declare_parameter_if_not_declared(
+    node, "service_introspection_mode", rclcpp::ParameterValue("disabled"));
+
   _motion_model = fromString(_motion_model_for_search);
   if (_motion_model == MotionModel::UNKNOWN) {
     RCLCPP_WARN(
@@ -361,10 +366,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   // Set starting point, in A* bin search coordinates
   float mx_start, my_start, mx_goal, my_goal;
   if (!costmap->worldToMapContinuous(
-    start.pose.position.x,
-    start.pose.position.y,
-    mx_start,
-    my_start))
+      start.pose.position.x,
+      start.pose.position.y,
+      mx_start,
+      my_start))
   {
     throw nav2_core::StartOutsideMapBounds(
             "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " +
@@ -383,10 +388,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
 
   // Set goal point, in A* bin search coordinates
   if (!costmap->worldToMapContinuous(
-    goal.pose.position.x,
-    goal.pose.position.y,
-    mx_goal,
-    my_goal))
+      goal.pose.position.x,
+      goal.pose.position.y,
+      mx_goal,
+      my_goal))
   {
     throw nav2_core::GoalOutsideMapBounds(
             "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " +
diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
index 82bdcc9810e..c3ad6365461 100644
--- a/nav2_smac_planner/src/smac_planner_lattice.cpp
+++ b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -288,10 +288,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
   // Set starting point, in A* bin search coordinates
   float mx_start, my_start, mx_goal, my_goal;
   if (!_costmap->worldToMapContinuous(
-    start.pose.position.x,
-    start.pose.position.y,
-    mx_start,
-    my_start))
+      start.pose.position.x,
+      start.pose.position.y,
+      mx_start,
+      my_start))
   {
     throw nav2_core::StartOutsideMapBounds(
             "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " +
@@ -303,10 +303,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
 
   // Set goal point, in A* bin search coordinates
   if (!_costmap->worldToMapContinuous(
-    goal.pose.position.x,
-    goal.pose.position.y,
-    mx_goal,
-    my_goal))
+      goal.pose.position.x,
+      goal.pose.position.y,
+      mx_goal,
+      my_goal))
   {
     throw nav2_core::GoalOutsideMapBounds(
             "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " +
diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp
index 4372c10127d..be417fa1be7 100644
--- a/nav2_smac_planner/src/smoother.cpp
+++ b/nav2_smac_planner/src/smoother.cpp
@@ -184,7 +184,7 @@ bool Smoother::smoothImpl(
     last_path = new_path;
   }
 
-  // Lets do additional refinement, it shouldn't take more than a couple milliseconds
+  // Let's do additional refinement, it shouldn't take more than a couple milliseconds
   // but really puts the path quality over the top.
   if (do_refinement_ && refinement_ctr_ < refinement_num_) {
     refinement_ctr_++;
diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp
index 53f9302b6f4..df8223ef9b5 100644
--- a/nav2_smac_planner/test/test_a_star.cpp
+++ b/nav2_smac_planner/test/test_a_star.cpp
@@ -427,7 +427,7 @@ TEST(AStarTest, test_constants)
   EXPECT_EQ(nav2_smac_planner::fromString("NONE"), nav2_smac_planner::MotionModel::UNKNOWN);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_collision_checker.cpp b/nav2_smac_planner/test/test_collision_checker.cpp
index 9c039e159d9..e6300feb628 100644
--- a/nav2_smac_planner/test/test_collision_checker.cpp
+++ b/nav2_smac_planner/test/test_collision_checker.cpp
@@ -215,7 +215,7 @@ TEST(collision_footprint, test_point_and_line_cost)
   delete costmap_;
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_costmap_downsampler.cpp b/nav2_smac_planner/test/test_costmap_downsampler.cpp
index 3011699b854..504ee9d3065 100644
--- a/nav2_smac_planner/test/test_costmap_downsampler.cpp
+++ b/nav2_smac_planner/test/test_costmap_downsampler.cpp
@@ -58,7 +58,7 @@ TEST(CostmapDownsampler, costmap_downsample_test)
   downsampler.resizeCostmap();
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp
index 4e9b39be67b..816376b814b 100644
--- a/nav2_smac_planner/test/test_node2d.cpp
+++ b/nav2_smac_planner/test/test_node2d.cpp
@@ -148,7 +148,7 @@ TEST(Node2DTest, test_node_2d_neighbors)
   EXPECT_EQ(neighbors.size(), 0u);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_nodebasic.cpp b/nav2_smac_planner/test/test_nodebasic.cpp
index c9435be2bd4..cd9ea624492 100644
--- a/nav2_smac_planner/test/test_nodebasic.cpp
+++ b/nav2_smac_planner/test/test_nodebasic.cpp
@@ -43,7 +43,7 @@ TEST(NodeBasicTest, test_node_basic)
   EXPECT_EQ(node3.graph_node_ptr, nullptr);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp
index 18dd18260a7..f7a83465859 100644
--- a/nav2_smac_planner/test/test_nodehybrid.cpp
+++ b/nav2_smac_planner/test/test_nodehybrid.cpp
@@ -411,7 +411,7 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test)
   }
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp
index 7c6fbe88566..e226356b227 100644
--- a/nav2_smac_planner/test/test_nodelattice.cpp
+++ b/nav2_smac_planner/test/test_nodelattice.cpp
@@ -388,7 +388,7 @@ TEST(NodeLatticeTest, test_node_lattice_custom_footprint)
   delete costmap;
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp
index 35ef1a90d12..5d035940fb8 100644
--- a/nav2_smac_planner/test/test_smac_2d.cpp
+++ b/nav2_smac_planner/test/test_smac_2d.cpp
@@ -143,7 +143,7 @@ TEST(SmacTest, test_smac_2d_reconfigure) {
     results);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp
index f269e9599c5..4f521fa71b1 100644
--- a/nav2_smac_planner/test/test_smac_hybrid.cpp
+++ b/nav2_smac_planner/test/test_smac_hybrid.cpp
@@ -163,7 +163,7 @@ TEST(SmacTest, test_smac_se2_reconfigure)
   EXPECT_EQ(nodeSE2->get_parameter("resolution").as_double(), 0.2);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp
index 9431776fc15..a1450c374bf 100644
--- a/nav2_smac_planner/test/test_smac_lattice.cpp
+++ b/nav2_smac_planner/test/test_smac_lattice.cpp
@@ -144,13 +144,13 @@ TEST(SmacTest, test_smac_lattice_reconfigure)
   } catch (...) {
   }
 
-  // So instead, lets call manually on a change
+  // So instead, let's call manually on a change
   std::vector<rclcpp::Parameter> parameters;
   parameters.push_back(rclcpp::Parameter("test.lattice_filepath", std::string("HI")));
   EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp
index 44c31900763..bc655189738 100644
--- a/nav2_smac_planner/test/test_smoother.cpp
+++ b/nav2_smac_planner/test/test_smoother.cpp
@@ -168,12 +168,19 @@ TEST(SmootherTest, test_full_smoother)
   double max_no_time = 0.0;
   EXPECT_FALSE(smoother->smooth(plan, costmap, max_no_time));
 
-  // Failure mode: Path is in collision, do 2x to exercise overlapping point
-  // attempts to update orientation should also fail
-  pose.pose.position.x = 1.25;
-  pose.pose.position.y = 1.25;
-  plan.poses.push_back(pose);
-  plan.poses.push_back(pose);
+  // Failure mode: invalid path and invalid orientation
+  // make the end of the path invalid
+  geometry_msgs::msg::PoseStamped lastPose = plan.poses.back();
+  unsigned int mx, my;
+  costmap->worldToMap(lastPose.pose.position.x, lastPose.pose.position.y, mx, my);
+  for (unsigned int i = mx - 5; i <= mx + 5; ++i) {
+    for (unsigned int j = my - 5; j <= my + 5; ++j) {
+      costmap->setCost(i, j, 254);
+    }
+  }
+
+  // duplicate last pose to make the orientation update fail
+  plan.poses.push_back(lastPose);
   EXPECT_FALSE(smoother->smooth(plan, costmap, maxtime));
   EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.z, 1.0, 1e-3);
   EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.x, 0.0, 1e-3);
@@ -184,7 +191,7 @@ TEST(SmootherTest, test_full_smoother)
   nav2_smac_planner::NodeHybrid::destroyStaticAssets();
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smoother/README.md b/nav2_smoother/README.md
index 04498a0034d..a62abd4b624 100644
--- a/nav2_smoother/README.md
+++ b/nav2_smoother/README.md
@@ -4,7 +4,7 @@ The Nav2 smoother is a Task Server in Nav2 that implements the `nav2_behavior_tr
 
 A smoothing module implementing the `nav2_behavior_tree::SmoothPath` interface is responsible for improving path smoothness and/or quality, typically given an unsmoothed path from the planner module in `nav2_planner`. It loads a map of potential smoother plugins to do the path smoothing in different user-defined situations.
 
-See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available smoother plugins. 
+See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available smoother plugins.
 
 See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-smoother-server.html) for additional parameter descriptions.
 
diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp
index 2da176b12f3..ff2551fbb22 100644
--- a/nav2_smoother/src/savitzky_golay_smoother.cpp
+++ b/nav2_smoother/src/savitzky_golay_smoother.cpp
@@ -119,7 +119,7 @@ bool SavitzkyGolaySmoother::smoothImpl(
 
   auto applyFilterOverAxes =
     [&](std::vector<geometry_msgs::msg::PoseStamped> & plan_pts,
-    const std::vector<geometry_msgs::msg::PoseStamped> & init_plan_pts) -> void
+      const std::vector<geometry_msgs::msg::PoseStamped> & init_plan_pts) -> void
     {
       auto pt_m3 = init_plan_pts[0].pose.position;
       auto pt_m2 = init_plan_pts[0].pose.position;
@@ -151,7 +151,7 @@ bool SavitzkyGolaySmoother::smoothImpl(
   const auto initial_path_poses = path.poses;
   applyFilterOverAxes(path.poses, initial_path_poses);
 
-  // Lets do additional refinement, it shouldn't take more than a couple milliseconds
+  // Let's do additional refinement, it shouldn't take more than a couple milliseconds
   if (do_refinement_) {
     for (int i = 0; i < refinement_num_; i++) {
       const auto reined_initial_path_poses = path.poses;
diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp
index 5b4c2fff6e0..d1957e38e35 100644
--- a/nav2_smoother/src/simple_smoother.cpp
+++ b/nav2_smoother/src/simple_smoother.cpp
@@ -184,7 +184,7 @@ void SimpleSmoother::smoothImpl(
     last_path = new_path;
   }
 
-  // Lets do additional refinement, it shouldn't take more than a couple milliseconds
+  // Let's do additional refinement, it shouldn't take more than a couple milliseconds
   // but really puts the path quality over the top.
   if (do_refinement_ && refinement_ctr_ < refinement_num_) {
     refinement_ctr_++;
diff --git a/nav2_smoother/test/test_savitzky_golay_smoother.cpp b/nav2_smoother/test/test_savitzky_golay_smoother.cpp
index 4ffb4520260..c3395c4d102 100644
--- a/nav2_smoother/test/test_savitzky_golay_smoother.cpp
+++ b/nav2_smoother/test/test_savitzky_golay_smoother.cpp
@@ -320,7 +320,7 @@ TEST(SmootherTest, test_sg_smoother_reversing)
   EXPECT_LT(length, base_length);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp
index 14291ac3a1f..3c8297c6ec5 100644
--- a/nav2_smoother/test/test_simple_smoother.cpp
+++ b/nav2_smoother/test/test_simple_smoother.cpp
@@ -278,7 +278,7 @@ TEST(SmootherTest, test_simple_smoother)
   EXPECT_TRUE(smoother->smooth(max_its_path, max_time));
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_system_tests/maps/map_circular.yaml b/nav2_system_tests/maps/map_circular.yaml
index 3a479308a73..4e5acec88f1 100644
--- a/nav2_system_tests/maps/map_circular.yaml
+++ b/nav2_system_tests/maps/map_circular.yaml
@@ -5,4 +5,3 @@ negate: 0
 occupied_thresh: 0.65
 free_thresh: 0.196
 map_type: occupancy
-
diff --git a/nav2_system_tests/models/cardboard_box.sdf b/nav2_system_tests/models/cardboard_box.sdf
index 97a40603c21..28d5a3ea1cb 100644
--- a/nav2_system_tests/models/cardboard_box.sdf
+++ b/nav2_system_tests/models/cardboard_box.sdf
@@ -6,4 +6,4 @@
       </uri>
       </include>
     </model>
-</sdf>
\ No newline at end of file
+</sdf>
diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml
index 05046c8d8a3..684dfd6800f 100644
--- a/nav2_system_tests/package.xml
+++ b/nav2_system_tests/package.xml
@@ -32,6 +32,7 @@
   <test_depend>nav2_util</test_depend>
   <test_depend>nav_msgs</test_depend>
   <test_depend>navigation2</test_depend>
+  <test_depend>python3-zmq</test_depend>
   <test_depend>rclcpp</test_depend>
   <test_depend>rclcpp_lifecycle</test_depend>
   <test_depend>rclpy</test_depend>
diff --git a/nav2_system_tests/scripts/ctest_loop.bash b/nav2_system_tests/scripts/ctest_loop.bash
index 29f0e278289..a074b698d0a 100755
--- a/nav2_system_tests/scripts/ctest_loop.bash
+++ b/nav2_system_tests/scripts/ctest_loop.bash
@@ -47,4 +47,3 @@ for ((i=1; i<=$loopcount; i++))
   done
 echo $failcount " FAILURES / " $loopcount
 echo $failcount " FAILURES / " $loopcount >> $outfile
-
diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py
index ece57eefed7..c33f1711bd2 100755
--- a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py
+++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py
@@ -18,23 +18,16 @@
 import sys
 
 from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import (
-    AppendEnvironmentVariable,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    SetEnvironmentVariable,
-)
+from launch import LaunchDescription, LaunchService
+from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription,
+                            SetEnvironmentVariable)
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_testing.legacy import LaunchTestService
-
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
 
@@ -50,7 +43,7 @@ def generate_launch_description():
     bt_navigator_xml = os.path.join(
         get_package_share_directory('nav2_bt_navigator'),
         'behavior_trees',
-        os.getenv('BT_NAVIGATOR_XML'),
+        os.getenv('BT_NAVIGATOR_XML', ''),
     )
 
     params_file = os.path.join(nav2_bringup_dir, 'params/nav2_params.yaml')
@@ -116,10 +109,10 @@ def generate_launch_description():
     )
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
-    testExecutable = os.getenv('TEST_EXECUTABLE')
+    testExecutable = os.getenv('TEST_EXECUTABLE', '')
 
     test1_action = ExecuteProcess(
         cmd=[testExecutable], name='test_assisted_teleop_behavior_node', output='screen'
diff --git a/nav2_system_tests/src/behaviors/backup/backup_tester.py b/nav2_system_tests/src/behaviors/backup/backup_tester.py
index ad6f724bf8f..bcd863cae0e 100755
--- a/nav2_system_tests/src/behaviors/backup/backup_tester.py
+++ b/nav2_system_tests/src/behaviors/backup/backup_tester.py
@@ -15,25 +15,24 @@
 
 import sys
 import time
+from typing import Optional
 
 from action_msgs.msg import GoalStatus
 from geometry_msgs.msg import Point32, PolygonStamped
 from nav2_msgs.action import BackUp
 from nav2_msgs.msg import Costmap
 from nav2_msgs.srv import ManageLifecycleNodes
-
 import rclpy
-
-from rclpy.action import ActionClient
+from rclpy.action import ActionClient  # type: ignore[attr-defined]
+from rclpy.action.client import ClientGoalHandle
 from rclpy.duration import Duration
 from rclpy.node import Node
-from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
-from rclpy.qos import QoSProfile
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
 
 
 class BackupTest(Node):
 
-    def __init__(self):
+    def __init__(self) -> None:
         super().__init__(node_name='backup_tester', namespace='')
         self.costmap_qos = QoSProfile(
             durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
@@ -46,10 +45,11 @@ def __init__(self):
             Costmap, 'local_costmap/costmap_raw', self.costmap_qos)
         self.footprint_pub = self.create_publisher(
             PolygonStamped, 'local_costmap/published_footprint', 10)
-        self.goal_handle = None
-        self.action_result = None
+        self.goal_handle: Optional[ClientGoalHandle[
+                BackUp.Goal, BackUp.Result, BackUp.Feedback]] = None
+        self.action_result = BackUp.Result()
 
-    def sendCommand(self, command):
+    def sendCommand(self, command: BackUp.Goal) -> bool:
         self.info_msg('Sending goal request...')
         self.goal_future = self.action_client.send_goal_async(command)
         try:
@@ -58,7 +58,7 @@ def sendCommand(self, command):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -68,8 +68,8 @@ def sendCommand(self, command):
         self.info_msg("Waiting for 'Backup' action to complete")
         try:
             rclpy.spin_until_future_complete(self, self.result_future)
-            status = self.result_future.result().status
-            result = self.result_future.result().result
+            status = self.result_future.result().status  # type: ignore[union-attr]
+            result = self.result_future.result().result  # type: ignore[union-attr]
             self.action_result = result
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
@@ -83,7 +83,7 @@ def sendCommand(self, command):
         self.info_msg('Backup failed to meet target!')
         return False
 
-    def sendAndPreemptWithFasterCommand(self, command):
+    def sendAndPreemptWithFasterCommand(self, command: BackUp.Goal) -> bool:
         # Send initial goal
         self.info_msg('Sending goal request...')
         self.goal_future = self.action_client.send_goal_async(command)
@@ -93,7 +93,7 @@ def sendAndPreemptWithFasterCommand(self, command):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -111,7 +111,7 @@ def sendAndPreemptWithFasterCommand(self, command):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error_msg('Preemption rejected')
             return False
 
@@ -122,8 +122,8 @@ def sendAndPreemptWithFasterCommand(self, command):
         self.info_msg("Waiting for 'backup' action Preemption to complete")
         try:
             rclpy.spin_until_future_complete(self, self.result_future)
-            status = self.result_future.result().status
-            result = self.result_future.result().result
+            status = self.result_future.result().status  # type: ignore[union-attr]
+            result = self.result_future.result().result  # type: ignore[union-attr]
             self.action_result = result
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
@@ -137,7 +137,7 @@ def sendAndPreemptWithFasterCommand(self, command):
         self.info_msg('Backup failed to meet target!')
         return False
 
-    def sendAndCancelCommand(self, command):
+    def sendAndCancelCommand(self, command: BackUp.Goal) -> bool:
         self.info_msg('Sending goal request...')
         self.goal_future = self.action_client.send_goal_async(command)
         try:
@@ -146,7 +146,7 @@ def sendAndCancelCommand(self, command):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -158,7 +158,7 @@ def sendAndCancelCommand(self, command):
         cancel_future = self.goal_handle.cancel_goal_async()
         rclpy.spin_until_future_complete(self, cancel_future)
         rclpy.spin_until_future_complete(self, self.result_future)
-        status = self.result_future.result().status
+        status = self.result_future.result().status  # type: ignore[union-attr]
         if status != GoalStatus.STATUS_CANCELED:
             self.info_msg(f'Goal failed with status code: {status}')
             return False
@@ -166,7 +166,7 @@ def sendAndCancelCommand(self, command):
             self.info_msg('Goal was canceled successfully')
             return True
 
-    def sendFreeCostmap(self):
+    def sendFreeCostmap(self) -> None:
         costmap_msg = Costmap()
         costmap_msg.header.frame_id = 'odom'
         costmap_msg.header.stamp = self.get_clock().now().to_msg()
@@ -189,7 +189,7 @@ def sendFreeCostmap(self):
         ]
         self.footprint_pub.publish(footprint_msg)
 
-    def sendOccupiedCostmap(self):
+    def sendOccupiedCostmap(self) -> None:
         costmap_msg = Costmap()
         costmap_msg.header.frame_id = 'odom'
         costmap_msg.header.stamp = self.get_clock().now().to_msg()
@@ -212,7 +212,7 @@ def sendOccupiedCostmap(self):
         ]
         self.footprint_pub.publish(footprint_msg)
 
-    def run(self):
+    def run(self) -> bool:
         while not self.action_client.wait_for_server(timeout_sec=1.0):
             self.info_msg("'Backup' action server not available, waiting...")
 
@@ -281,7 +281,7 @@ def run(self):
             self.info_msg('Test D passed')
         return True
 
-    def shutdown(self):
+    def shutdown(self) -> None:
         self.info_msg('Shutting down')
 
         self.action_client.destroy()
@@ -303,17 +303,17 @@ def shutdown(self):
 
         self.info_msg(f'{transition_service} finished')
 
-    def info_msg(self, msg: str):
+    def info_msg(self, msg: str) -> None:
         self.get_logger().info(msg)
 
-    def warn_msg(self, msg: str):
+    def warn_msg(self, msg: str) -> None:
         self.get_logger().warn(msg)
 
-    def error_msg(self, msg: str):
+    def error_msg(self, msg: str) -> None:
         self.get_logger().error(msg)
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     rclpy.init()
     time.sleep(10)
     test = BackupTest()
diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py
index c8696a6ad4f..291dbb4b836 100755
--- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py
+++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py
@@ -18,15 +18,9 @@
 import sys
 
 from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    SetEnvironmentVariable)
+from launch import LaunchDescription, LaunchService
+from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
+                            IncludeLaunchDescription, SetEnvironmentVariable)
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
@@ -34,7 +28,7 @@
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
     params_file = LaunchConfiguration('params_file')
@@ -139,12 +133,13 @@ def generate_launch_description():
     )
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
     test1_action = ExecuteProcess(
         cmd=[os.path.join(
-            os.getenv('TEST_DIR'), 'backup_tester.py'), '--ros-args', '-p', 'use_sim_time:=True'],
+            os.getenv('TEST_DIR', ''),
+            'backup_tester.py'), '--ros-args', '-p', 'use_sim_time:=True'],
         name='tester_node',
         output='screen',
     )
diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py b/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py
index 2f057620bac..2f3721c1580 100755
--- a/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py
+++ b/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py
@@ -15,25 +15,24 @@
 
 import sys
 import time
+from typing import Optional
 
 from action_msgs.msg import GoalStatus
 from geometry_msgs.msg import Point32, PolygonStamped
 from nav2_msgs.action import DriveOnHeading
 from nav2_msgs.msg import Costmap
 from nav2_msgs.srv import ManageLifecycleNodes
-
 import rclpy
-
-from rclpy.action import ActionClient
+from rclpy.action import ActionClient  # type: ignore[attr-defined]
+from rclpy.action.client import ClientGoalHandle
 from rclpy.duration import Duration
 from rclpy.node import Node
-from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
-from rclpy.qos import QoSProfile
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
 
 
 class DriveTest(Node):
 
-    def __init__(self):
+    def __init__(self) -> None:
         super().__init__(node_name='drive_tester', namespace='')
         self.costmap_qos = QoSProfile(
             durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
@@ -46,10 +45,12 @@ def __init__(self):
             Costmap, 'local_costmap/costmap_raw', self.costmap_qos)
         self.footprint_pub = self.create_publisher(
             PolygonStamped, 'local_costmap/published_footprint', 10)
-        self.goal_handle = None
-        self.action_result = None
+        self.goal_handle: Optional[ClientGoalHandle[
+                DriveOnHeading.Goal, DriveOnHeading.Result,
+                DriveOnHeading.Feedback]] = None
+        self.action_result = DriveOnHeading.Result()
 
-    def sendCommand(self, command):
+    def sendCommand(self, command: DriveOnHeading.Goal) -> bool:
         self.info_msg('Sending goal request...')
         self.goal_future = self.action_client.send_goal_async(command)
         try:
@@ -58,7 +59,7 @@ def sendCommand(self, command):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -68,8 +69,8 @@ def sendCommand(self, command):
         self.info_msg("Waiting for 'DriveOnHeading' action to complete")
         try:
             rclpy.spin_until_future_complete(self, self.result_future)
-            status = self.result_future.result().status
-            result = self.result_future.result().result
+            status = self.result_future.result().status  # type: ignore[union-attr]
+            result = self.result_future.result().result  # type: ignore[union-attr]
             self.action_result = result
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
@@ -83,7 +84,7 @@ def sendCommand(self, command):
         self.info_msg('DriveOnHeading failed to meet target!')
         return False
 
-    def sendAndPreemptWithFasterCommand(self, command):
+    def sendAndPreemptWithFasterCommand(self, command: DriveOnHeading.Goal) -> bool:
         # Send initial goal
         self.info_msg('Sending goal request...')
         self.goal_future = self.action_client.send_goal_async(command)
@@ -93,7 +94,7 @@ def sendAndPreemptWithFasterCommand(self, command):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -111,7 +112,7 @@ def sendAndPreemptWithFasterCommand(self, command):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error_msg('Preemption rejected')
             return False
 
@@ -122,8 +123,8 @@ def sendAndPreemptWithFasterCommand(self, command):
         self.info_msg("Waiting for 'DriveOnHeading' action Preemption to complete")
         try:
             rclpy.spin_until_future_complete(self, self.result_future)
-            status = self.result_future.result().status
-            result = self.result_future.result().result
+            status = self.result_future.result().status  # type: ignore[union-attr]
+            result = self.result_future.result().result  # type: ignore[union-attr]
             self.action_result = result
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
@@ -137,7 +138,7 @@ def sendAndPreemptWithFasterCommand(self, command):
         self.info_msg('DriveOnHeading failed to meet target!')
         return False
 
-    def sendAndCancelCommand(self, command):
+    def sendAndCancelCommand(self, command: DriveOnHeading.Goal) -> bool:
         self.info_msg('Sending goal request...')
         self.goal_future = self.action_client.send_goal_async(command)
         try:
@@ -146,7 +147,7 @@ def sendAndCancelCommand(self, command):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -158,7 +159,7 @@ def sendAndCancelCommand(self, command):
         cancel_future = self.goal_handle.cancel_goal_async()
         rclpy.spin_until_future_complete(self, cancel_future)
         rclpy.spin_until_future_complete(self, self.result_future)
-        status = self.result_future.result().status
+        status = self.result_future.result().status  # type: ignore[union-attr]
         if status != GoalStatus.STATUS_CANCELED:
             self.info_msg(f'Goal failed with status code: {status}')
             return False
@@ -166,7 +167,7 @@ def sendAndCancelCommand(self, command):
             self.info_msg('Goal was canceled successfully')
             return True
 
-    def sendFreeCostmap(self):
+    def sendFreeCostmap(self) -> None:
         costmap_msg = Costmap()
         costmap_msg.header.frame_id = 'odom'
         costmap_msg.header.stamp = self.get_clock().now().to_msg()
@@ -189,7 +190,7 @@ def sendFreeCostmap(self):
         ]
         self.footprint_pub.publish(footprint_msg)
 
-    def sendOccupiedCostmap(self):
+    def sendOccupiedCostmap(self) -> None:
         costmap_msg = Costmap()
         costmap_msg.header.frame_id = 'odom'
         costmap_msg.header.stamp = self.get_clock().now().to_msg()
@@ -212,7 +213,7 @@ def sendOccupiedCostmap(self):
         ]
         self.footprint_pub.publish(footprint_msg)
 
-    def run(self):
+    def run(self) -> bool:
         while not self.action_client.wait_for_server(timeout_sec=1.0):
             self.info_msg("'DriveOnHeading' action server not available, waiting...")
 
@@ -289,7 +290,7 @@ def run(self):
             self.info_msg('Test D passed')
         return True
 
-    def shutdown(self):
+    def shutdown(self) -> None:
         self.info_msg('Shutting down')
 
         self.action_client.destroy()
@@ -311,17 +312,17 @@ def shutdown(self):
 
         self.info_msg(f'{transition_service} finished')
 
-    def info_msg(self, msg: str):
+    def info_msg(self, msg: str) -> None:
         self.get_logger().info(msg)
 
-    def warn_msg(self, msg: str):
+    def warn_msg(self, msg: str) -> None:
         self.get_logger().warn(msg)
 
-    def error_msg(self, msg: str):
+    def error_msg(self, msg: str) -> None:
         self.get_logger().error(msg)
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     rclpy.init()
     time.sleep(10)
     test = DriveTest()
diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py
index 232e5a8c093..016d9ef163f 100755
--- a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py
+++ b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py
@@ -18,15 +18,9 @@
 import sys
 
 from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    SetEnvironmentVariable)
+from launch import LaunchDescription, LaunchService
+from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
+                            IncludeLaunchDescription, SetEnvironmentVariable)
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
@@ -34,7 +28,7 @@
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
     params_file = LaunchConfiguration('params_file')
@@ -139,12 +133,13 @@ def generate_launch_description():
     )
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
     test1_action = ExecuteProcess(
         cmd=[os.path.join(
-            os.getenv('TEST_DIR'), 'drive_tester.py'), '--ros-args', '-p', 'use_sim_time:=True'],
+            os.getenv('TEST_DIR', ''),
+            'drive_tester.py'), '--ros-args', '-p', 'use_sim_time:=True'],
         name='tester_node',
         output='screen',
     )
diff --git a/nav2_system_tests/src/behaviors/spin/spin_tester.py b/nav2_system_tests/src/behaviors/spin/spin_tester.py
index b2d6941987f..ff5087e62ed 100755
--- a/nav2_system_tests/src/behaviors/spin/spin_tester.py
+++ b/nav2_system_tests/src/behaviors/spin/spin_tester.py
@@ -15,25 +15,24 @@
 
 import sys
 import time
+from typing import Optional
 
 from action_msgs.msg import GoalStatus
 from geometry_msgs.msg import Point32, PolygonStamped
 from nav2_msgs.action import Spin
 from nav2_msgs.msg import Costmap
 from nav2_msgs.srv import ManageLifecycleNodes
-
 import rclpy
-
-from rclpy.action import ActionClient
+from rclpy.action import ActionClient  # type: ignore[attr-defined]
+from rclpy.action.client import ClientGoalHandle
 from rclpy.duration import Duration
 from rclpy.node import Node
-from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
-from rclpy.qos import QoSProfile
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
 
 
 class SpinTest(Node):
 
-    def __init__(self):
+    def __init__(self) -> None:
         super().__init__(node_name='spin_tester', namespace='')
         self.costmap_qos = QoSProfile(
             durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
@@ -46,10 +45,11 @@ def __init__(self):
             Costmap, 'local_costmap/costmap_raw', self.costmap_qos)
         self.footprint_pub = self.create_publisher(
             PolygonStamped, 'local_costmap/published_footprint', 10)
-        self.goal_handle = None
-        self.action_result = None
+        self.goal_handle: Optional[ClientGoalHandle[
+                Spin.Goal, Spin.Result, Spin.Feedback]] = None
+        self.action_result = Spin.Result()
 
-    def sendCommand(self, command):
+    def sendCommand(self, command: Spin.Goal) -> bool:
         self.info_msg('Sending goal request...')
         self.goal_future = self.action_client.send_goal_async(command)
         try:
@@ -58,7 +58,7 @@ def sendCommand(self, command):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -68,8 +68,8 @@ def sendCommand(self, command):
         self.info_msg("Waiting for 'spin' action to complete")
         try:
             rclpy.spin_until_future_complete(self, self.result_future)
-            status = self.result_future.result().status
-            result = self.result_future.result().result
+            status = self.result_future.result().status  # type: ignore[union-attr]
+            result = self.result_future.result().result  # type: ignore[union-attr]
             self.action_result = result
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
@@ -83,7 +83,7 @@ def sendCommand(self, command):
         self.info_msg('Spin failed to meet target!')
         return False
 
-    def sendAndPreemptWithInvertedCommand(self, command):
+    def sendAndPreemptWithInvertedCommand(self, command: Spin.Goal) -> bool:
         # Send initial goal
         self.info_msg('Sending goal request...')
         self.goal_future = self.action_client.send_goal_async(command)
@@ -93,7 +93,7 @@ def sendAndPreemptWithInvertedCommand(self, command):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -111,7 +111,7 @@ def sendAndPreemptWithInvertedCommand(self, command):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error_msg('Preemption rejected')
             return False
 
@@ -122,8 +122,8 @@ def sendAndPreemptWithInvertedCommand(self, command):
         self.info_msg("Waiting for 'spin' action Preemption to complete")
         try:
             rclpy.spin_until_future_complete(self, self.result_future)
-            status = self.result_future.result().status
-            result = self.result_future.result().result
+            status = self.result_future.result().status  # type: ignore[union-attr]
+            result = self.result_future.result().result  # type: ignore[union-attr]
             self.action_result = result
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
@@ -137,7 +137,7 @@ def sendAndPreemptWithInvertedCommand(self, command):
         self.info_msg('Spin failed to meet target!')
         return False
 
-    def sendAndCancelCommand(self, command):
+    def sendAndCancelCommand(self, command: Spin.Goal) -> bool:
         self.info_msg('Sending goal request...')
         self.goal_future = self.action_client.send_goal_async(command)
         try:
@@ -146,7 +146,7 @@ def sendAndCancelCommand(self, command):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -158,7 +158,7 @@ def sendAndCancelCommand(self, command):
         cancel_future = self.goal_handle.cancel_goal_async()
         rclpy.spin_until_future_complete(self, cancel_future)
         rclpy.spin_until_future_complete(self, self.result_future)
-        status = self.result_future.result().status
+        status = self.result_future.result().status  # type: ignore[union-attr]
         if status != GoalStatus.STATUS_CANCELED:
             self.info_msg(f'Goal failed with status code: {status}')
             return False
@@ -166,7 +166,7 @@ def sendAndCancelCommand(self, command):
             self.info_msg('Goal was canceled successfully')
             return True
 
-    def sendFreeCostmap(self):
+    def sendFreeCostmap(self) -> None:
         costmap_msg = Costmap()
         costmap_msg.header.frame_id = 'odom'
         costmap_msg.header.stamp = self.get_clock().now().to_msg()
@@ -189,7 +189,7 @@ def sendFreeCostmap(self):
         ]
         self.footprint_pub.publish(footprint_msg)
 
-    def sendOccupiedCostmap(self):
+    def sendOccupiedCostmap(self) -> None:
         costmap_msg = Costmap()
         costmap_msg.header.frame_id = 'odom'
         costmap_msg.header.stamp = self.get_clock().now().to_msg()
@@ -212,7 +212,7 @@ def sendOccupiedCostmap(self):
         ]
         self.footprint_pub.publish(footprint_msg)
 
-    def run(self):
+    def run(self) -> bool:
         while not self.action_client.wait_for_server(timeout_sec=1.0):
             self.info_msg("'spin' action server not available, waiting...")
 
@@ -273,7 +273,7 @@ def run(self):
             self.info_msg('Test D passed')
         return True
 
-    def shutdown(self):
+    def shutdown(self) -> None:
         self.info_msg('Shutting down')
 
         self.action_client.destroy()
@@ -295,17 +295,17 @@ def shutdown(self):
 
         self.info_msg(f'{transition_service} finished')
 
-    def info_msg(self, msg: str):
+    def info_msg(self, msg: str) -> None:
         self.get_logger().info(msg)
 
-    def warn_msg(self, msg: str):
+    def warn_msg(self, msg: str) -> None:
         self.get_logger().warn(msg)
 
-    def error_msg(self, msg: str):
+    def error_msg(self, msg: str) -> None:
         self.get_logger().error(msg)
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     rclpy.init()
     time.sleep(10)
     test = SpinTest()
diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py
index e1f7c991b1e..cadef8c367a 100755
--- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py
+++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py
@@ -18,15 +18,9 @@
 import sys
 
 from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    SetEnvironmentVariable)
+from launch import LaunchDescription, LaunchService
+from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess,
+                            IncludeLaunchDescription, SetEnvironmentVariable)
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
@@ -34,7 +28,7 @@
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
     params_file = LaunchConfiguration('params_file')
@@ -139,12 +133,13 @@ def generate_launch_description():
     )
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
     test1_action = ExecuteProcess(
         cmd=[os.path.join(
-            os.getenv('TEST_DIR'), 'spin_tester.py'), '--ros-args', '-p', 'use_sim_time:=True'],
+            os.getenv('TEST_DIR', ''),
+            'spin_tester.py'), '--ros-args', '-p', 'use_sim_time:=True'],
         name='tester_node',
         output='screen',
     )
diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py
index b13024b2d86..49cee4ea48e 100755
--- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py
+++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py
@@ -18,24 +18,17 @@
 import sys
 
 from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import (
-    AppendEnvironmentVariable,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    SetEnvironmentVariable,
-)
+from launch import LaunchDescription, LaunchService
+from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription,
+                            SetEnvironmentVariable)
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_testing.legacy import LaunchTestService
-
 from nav2_common.launch import RewrittenYaml
 from nav2_simple_commander.utils import kill_os_processes
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     ros_gz_sim_dir = get_package_share_directory('ros_gz_sim')
@@ -52,7 +45,7 @@ def generate_launch_description():
     bt_navigator_xml = os.path.join(
         get_package_share_directory('nav2_bt_navigator'),
         'behavior_trees',
-        os.getenv('BT_NAVIGATOR_XML'),
+        os.getenv('BT_NAVIGATOR_XML', ''),
     )
 
     bringup_dir = get_package_share_directory('nav2_bringup')
@@ -121,10 +114,10 @@ def generate_launch_description():
     )
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
-    testExecutable = os.getenv('TEST_EXECUTABLE')
+    testExecutable = os.getenv('TEST_EXECUTABLE', '')
 
     test1_action = ExecuteProcess(
         cmd=[testExecutable], name='test_wait_behavior_node', output='screen',
diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml
index 246810868ba..c763808912f 100644
--- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml
+++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml
@@ -106,7 +106,16 @@ controller_server:
       trans_stopped_velocity: 0.25
       short_circuit_trajectory_evaluation: True
       stateful: True
-      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      critics:
+        [
+          "RotateToGoal",
+          "Oscillation",
+          "BaseObstacle",
+          "GoalAlign",
+          "PathAlign",
+          "PathDist",
+          "GoalDist",
+        ]
       BaseObstacle.scale: 0.02
       PathAlign.scale: 32.0
       PathAlign.forward_point_distance: 0.1
@@ -309,18 +318,18 @@ docking_server:
     max_retries: 3
     base_frame: "base_link"
     fixed_frame: "odom"
-    dock_backwards: false
     dock_prestaging_tolerance: 0.5
 
     # Types of docks
-    dock_plugins: ['simple_charging_dock']
+    dock_plugins: ["simple_charging_dock"]
     simple_charging_dock:
-      plugin: 'opennav_docking::SimpleChargingDock'
+      plugin: "opennav_docking::SimpleChargingDock"
       docking_threshold: 0.05
       staging_x_offset: -0.7
       use_external_detection_pose: true
       use_battery_status: false # true
       use_stall_detection: false # true
+      dock_direction: "forward"
 
       external_detection_timeout: 1.0
       external_detection_translation_x: -0.18
diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml
index f1941796018..a7593657cee 100644
--- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml
+++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml
@@ -105,7 +105,16 @@ controller_server:
       trans_stopped_velocity: 0.25
       short_circuit_trajectory_evaluation: True
       stateful: True
-      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      critics:
+        [
+          "RotateToGoal",
+          "Oscillation",
+          "BaseObstacle",
+          "GoalAlign",
+          "PathAlign",
+          "PathDist",
+          "GoalDist",
+        ]
       BaseObstacle.scale: 0.02
       PathAlign.scale: 32.0
       PathAlign.forward_point_distance: 0.1
@@ -299,18 +308,18 @@ docking_server:
     max_retries: 3
     base_frame: "base_link"
     fixed_frame: "odom"
-    dock_backwards: false
     dock_prestaging_tolerance: 0.5
 
     # Types of docks
-    dock_plugins: ['simple_charging_dock']
+    dock_plugins: ["simple_charging_dock"]
     simple_charging_dock:
-      plugin: 'opennav_docking::SimpleChargingDock'
+      plugin: "opennav_docking::SimpleChargingDock"
       docking_threshold: 0.05
       staging_x_offset: -0.7
       use_external_detection_pose: true
       use_battery_status: false # true
       use_stall_detection: false # true
+      dock_direction: "forward"
 
       external_detection_timeout: 1.0
       external_detection_translation_x: -0.18
diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml
index 28c4acba800..aef69d68074 100644
--- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml
+++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml
@@ -105,7 +105,16 @@ controller_server:
       trans_stopped_velocity: 0.25
       short_circuit_trajectory_evaluation: True
       stateful: True
-      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      critics:
+        [
+          "RotateToGoal",
+          "Oscillation",
+          "BaseObstacle",
+          "GoalAlign",
+          "PathAlign",
+          "PathDist",
+          "GoalDist",
+        ]
       BaseObstacle.scale: 0.02
       PathAlign.scale: 32.0
       PathAlign.forward_point_distance: 0.1
@@ -299,18 +308,18 @@ docking_server:
     max_retries: 3
     base_frame: "base_link"
     fixed_frame: "odom"
-    dock_backwards: false
     dock_prestaging_tolerance: 0.5
 
     # Types of docks
-    dock_plugins: ['simple_charging_dock']
+    dock_plugins: ["simple_charging_dock"]
     simple_charging_dock:
-      plugin: 'opennav_docking::SimpleChargingDock'
+      plugin: "opennav_docking::SimpleChargingDock"
       docking_threshold: 0.05
       staging_x_offset: -0.7
       use_external_detection_pose: true
       use_battery_status: false # true
       use_stall_detection: false # true
+      dock_direction: "forward"
 
       external_detection_timeout: 1.0
       external_detection_translation_x: -0.18
diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py
index bb43001b5b9..71d4189994a 100755
--- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py
+++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py
@@ -20,24 +20,17 @@
 import sys
 
 from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import (
-    AppendEnvironmentVariable,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    SetEnvironmentVariable,
-)
+from launch import LaunchDescription, LaunchService
+from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription,
+                            SetEnvironmentVariable)
 from launch.launch_context import LaunchContext
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_testing.legacy import LaunchTestService
-
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     nav2_sys_test_dir = get_package_share_directory('nav2_system_tests')
@@ -50,12 +43,12 @@ def generate_launch_description():
         robot_description = infp.read()
 
     map_yaml_file = os.path.join(nav2_sys_test_dir, 'maps', 'map_circular.yaml')
-    filter_mask_file = os.getenv('TEST_MASK')
+    filter_mask_file = os.getenv('TEST_MASK', '')
 
     bt_navigator_xml = os.path.join(
         get_package_share_directory('nav2_bt_navigator'),
         'behavior_trees',
-        os.getenv('BT_NAVIGATOR_XML'),
+        os.getenv('BT_NAVIGATOR_XML', ''),
     )
 
     script_dir = os.path.dirname(os.path.realpath(__file__))
@@ -171,12 +164,12 @@ def generate_launch_description():
     )
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
     test1_action = ExecuteProcess(
         cmd=[
-            os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'),
+            os.path.join(os.getenv('TEST_DIR', ''), 'tester_node.py'),
             '-t',
             'keepout',
             '-r',
diff --git a/nav2_system_tests/src/costmap_filters/test_speed_launch.py b/nav2_system_tests/src/costmap_filters/test_speed_launch.py
index 281aed36250..8746c7c7266 100755
--- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py
+++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py
@@ -20,24 +20,17 @@
 import sys
 
 from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import (
-    AppendEnvironmentVariable,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    SetEnvironmentVariable,
-)
+from launch import LaunchDescription, LaunchService
+from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription,
+                            SetEnvironmentVariable)
 from launch.launch_context import LaunchContext
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_testing.legacy import LaunchTestService
-
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     nav2_sys_test_dir = get_package_share_directory('nav2_system_tests')
@@ -55,10 +48,10 @@ def generate_launch_description():
     bt_navigator_xml = os.path.join(
         get_package_share_directory('nav2_bt_navigator'),
         'behavior_trees',
-        os.getenv('BT_NAVIGATOR_XML'),
+        os.getenv('BT_NAVIGATOR_XML', ''),
     )
 
-    params_file = os.getenv('PARAMS_FILE')
+    params_file = os.getenv('PARAMS_FILE', '')
 
     # Replace the `use_astar` setting on the params file
     param_substitutions = {
@@ -162,12 +155,12 @@ def generate_launch_description():
     )
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
     test1_action = ExecuteProcess(
         cmd=[
-            os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'),
+            os.path.join(os.getenv('TEST_DIR', ''), 'tester_node.py'),
             '-t',
             'speed',
             '-r',
diff --git a/nav2_system_tests/src/costmap_filters/tester_node.py b/nav2_system_tests/src/costmap_filters/tester_node.py
index 3b3f723095b..0f80d146fe6 100755
--- a/nav2_system_tests/src/costmap_filters/tester_node.py
+++ b/nav2_system_tests/src/costmap_filters/tester_node.py
@@ -23,21 +23,16 @@
 from typing import Optional
 
 from action_msgs.msg import GoalStatus
-from geometry_msgs.msg import Pose
-from geometry_msgs.msg import PoseStamped
-from geometry_msgs.msg import PoseWithCovarianceStamped
+from geometry_msgs.msg import Pose, PoseStamped, PoseWithCovarianceStamped
 from lifecycle_msgs.srv import GetState
 from nav2_msgs.action import NavigateToPose
 from nav2_msgs.msg import SpeedLimit
 from nav2_msgs.srv import ManageLifecycleNodes
-from nav_msgs.msg import OccupancyGrid
-from nav_msgs.msg import Path
-
+from nav_msgs.msg import OccupancyGrid, Path
 import rclpy
-from rclpy.action import ActionClient
+from rclpy.action import ActionClient  # type: ignore[attr-defined]
 from rclpy.node import Node
-from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
-from rclpy.qos import QoSProfile
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
 from sensor_msgs.msg import PointCloud2
 
 
@@ -49,12 +44,12 @@ class TestType(Enum):
 class FilterMask:
 
     def __init__(self, filter_mask: OccupancyGrid):
-        self.filter_mask = filter_mask
+        self.filter_mask: OccupancyGrid = filter_mask
 
     # Converts world coordinates into filter mask map coordinate.
     # Returns filter mask map coordinates or (-1, -1) in case
     # if world coordinates are out of mask bounds.
-    def worldToMap(self, wx: float, wy: float):
+    def worldToMap(self, wx: float, wy: float) -> tuple[int, int]:
         origin_x = self.filter_mask.info.origin.position.x
         origin_y = self.filter_mask.info.origin.position.y
         size_x = self.filter_mask.info.width
@@ -73,7 +68,7 @@ def worldToMap(self, wx: float, wy: float):
         return -1, -1
 
     # Gets filter_mask[mx, my] value
-    def getValue(self, mx, my):
+    def getValue(self, mx: int, my: int):  # type: ignore[no-untyped-def]
         size_x = self.filter_mask.info.width
         return self.filter_mask.data[mx + my * size_x]
 
@@ -100,9 +95,9 @@ def __init__(
         )
 
         volatile_qos = QoSProfile(
-            durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE,
+            durability=QoSDurabilityPolicy.VOLATILE,
             reliability=QoSReliabilityPolicy.RELIABLE,
-            history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+            history=QoSHistoryPolicy.KEEP_LAST,
             depth=1,
         )
 
@@ -158,16 +153,16 @@ def __init__(
         self.goal_pose = goal_pose
         self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
 
-    def info_msg(self, msg: str):
+    def info_msg(self, msg: str) -> None:
         self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
 
-    def warn_msg(self, msg: str):
+    def warn_msg(self, msg: str) -> None:
         self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m')
 
-    def error_msg(self, msg: str):
+    def error_msg(self, msg: str) -> None:
         self.get_logger().error('\033[1;37;41m' + msg + '\033[0m')
 
-    def setInitialPose(self):
+    def setInitialPose(self) -> None:
         msg = PoseWithCovarianceStamped()
         msg.pose.pose = self.initial_pose
         msg.header.frame_id = 'map'
@@ -175,17 +170,17 @@ def setInitialPose(self):
         self.initial_pose_pub.publish(msg)
         self.currentPose = self.initial_pose
 
-    def getStampedPoseMsg(self, pose: Pose):
+    def getStampedPoseMsg(self, pose: Pose) -> PoseStamped:
         msg = PoseStamped()
         msg.header.frame_id = 'map'
         msg.pose = pose
         return msg
 
-    def publishGoalPose(self, goal_pose: Optional[Pose] = None):
+    def publishGoalPose(self, goal_pose: Optional[Pose] = None) -> None:
         self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
         self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose))
 
-    def runNavigateAction(self, goal_pose: Optional[Pose] = None):
+    def runNavigateAction(self, goal_pose: Optional[Pose] = None) -> bool:
         # Sends a `NavToPose` action request and waits for completion
         self.info_msg("Waiting for 'NavigateToPose' action server")
         while not self.action_client.wait_for_server(timeout_sec=1.0):
@@ -201,7 +196,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
         rclpy.spin_until_future_complete(self, send_goal_future)
         goal_handle = send_goal_future.result()
 
-        if not goal_handle.accepted:
+        if not goal_handle or not goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -210,7 +205,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
 
         self.info_msg("Waiting for 'NavigateToPose' action to complete")
         rclpy.spin_until_future_complete(self, get_result_future)
-        status = get_result_future.result().status
+        status = get_result_future.result().status  # type: ignore[union-attr]
         if status != GoalStatus.STATUS_SUCCEEDED:
             self.info_msg(f'Goal failed with status code: {status}')
             return False
@@ -218,7 +213,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
         self.info_msg('Goal succeeded!')
         return True
 
-    def isInKeepout(self, x, y):
+    def isInKeepout(self, x: float, y: float) -> bool:
         mx, my = self.filter_mask.worldToMap(x, y)
         if mx == -1 and my == -1:  # Out of mask's area
             return False
@@ -227,7 +222,7 @@ def isInKeepout(self, x, y):
         return False
 
     # Checks that (x, y) position does not belong to a keepout zone.
-    def checkKeepout(self, x, y):
+    def checkKeepout(self, x: float, y: float) -> bool:
         if not self.mask_received:
             self.warn_msg('Filter mask was not received')
         elif self.isInKeepout(x, y):
@@ -243,7 +238,7 @@ def checkKeepout(self, x, y):
     # Also verifies that speed limit messages received no more than N-times
     # (where N - is the length of 'limits' array),
     # otherwise sets overall 'filter_test_result' to be false.
-    def checkSpeed(self, it, speed_limit):
+    def checkSpeed(self, it: int, speed_limit: float) -> None:
         if it >= len(self.limits):
             self.error_msg('Got excess speed limit')
             self.filter_test_result = False
@@ -258,7 +253,7 @@ def checkSpeed(self, it, speed_limit):
                 + str(self.limits[it])
             )
 
-    def poseCallback(self, msg):
+    def poseCallback(self, msg: PoseWithCovarianceStamped) -> None:
         self.info_msg('Received amcl_pose')
         self.current_pose = msg.pose.pose
         self.initial_pose_received = True
@@ -268,41 +263,41 @@ def poseCallback(self, msg):
             ):
                 self.error_msg('Robot goes into keepout zone')
 
-    def planCallback(self, msg):
+    def planCallback(self, msg: Path) -> None:
         self.info_msg('Received plan')
         for pose in msg.poses:
             if not self.checkKeepout(pose.pose.position.x, pose.pose.position.y):
                 self.error_msg('Path plan intersects with keepout zone')
                 return
 
-    def clearingEndpointsCallback(self, msg):
+    def clearingEndpointsCallback(self, msg: PointCloud2) -> None:
         if len(msg.data) > 0:
             self.clearing_endpoints_received = True
 
-    def voxelMarkedCallback(self, msg):
+    def voxelMarkedCallback(self, msg: PointCloud2) -> None:
         if len(msg.data) > 0:
             self.voxel_marked_received = True
 
-    def voxelUnknownCallback(self, msg):
+    def voxelUnknownCallback(self, msg: PointCloud2) -> None:
         if len(msg.data) > 0:
             self.voxel_unknown_received = True
 
-    def dwbCostCloudCallback(self, msg):
+    def dwbCostCloudCallback(self, msg: PointCloud2) -> None:
         self.info_msg('Received cost_cloud points')
         if len(msg.data) > 0:
             self.cost_cloud_received = True
 
-    def speedLimitCallback(self, msg):
+    def speedLimitCallback(self, msg: SpeedLimit) -> None:
         self.info_msg(f'Received speed limit: {msg.speed_limit}')
         self.checkSpeed(self.speed_it, msg.speed_limit)
         self.speed_it += 1
 
-    def maskCallback(self, msg):
+    def maskCallback(self, msg: OccupancyGrid) -> None:
         self.info_msg('Received filter mask')
         self.filter_mask = FilterMask(msg)
         self.mask_received = True
 
-    def wait_for_filter_mask(self, timeout):
+    def wait_for_filter_mask(self, timeout: float) -> bool:
         start_time = time.time()
 
         while not self.mask_received:
@@ -313,7 +308,7 @@ def wait_for_filter_mask(self, timeout):
                 return False
         return True
 
-    def wait_for_pointcloud_subscribers(self, timeout):
+    def wait_for_pointcloud_subscribers(self, timeout: float) -> bool:
         start_time = time.time()
         while (
             not self.voxel_unknown_received
@@ -333,7 +328,7 @@ def wait_for_pointcloud_subscribers(self, timeout):
                 return False
         return True
 
-    def reachesGoal(self, timeout, distance):
+    def reachesGoal(self, timeout: float, distance: float) -> bool:
         goalReached = False
         start_time = time.time()
 
@@ -348,14 +343,17 @@ def reachesGoal(self, timeout, distance):
                     self.error_msg('Robot timed out reaching its goal!')
                     return False
 
-    def distanceFromGoal(self):
+        self.info_msg('Robot reached its goal!')
+        return True
+
+    def distanceFromGoal(self) -> float:
         d_x = self.current_pose.position.x - self.goal_pose.position.x
         d_y = self.current_pose.position.y - self.goal_pose.position.y
         distance = math.sqrt(d_x * d_x + d_y * d_y)
         self.info_msg(f'Distance from goal is: {distance}')
         return distance
 
-    def wait_for_node_active(self, node_name: str):
+    def wait_for_node_active(self, node_name: str) -> None:
         # Waits for the node within the tester namespace to become active
         self.info_msg(f'Waiting for {node_name} to become active')
         node_service = f'{node_name}/get_state'
@@ -369,15 +367,15 @@ def wait_for_node_active(self, node_name: str):
             future = state_client.call_async(req)
             rclpy.spin_until_future_complete(self, future)
             if future.result() is not None:
-                state = future.result().current_state.label
+                state = future.result().current_state.label  # type: ignore[union-attr]
                 self.info_msg(f'Result of get_state: {state}')
             else:
                 self.error_msg(
-                    'Exception while calling service: %r' % future.exception()
+                    f'Exception while calling service: {future.exception()!r}'
                 )
             time.sleep(5)
 
-    def shutdown(self):
+    def shutdown(self) -> None:
         self.info_msg('Shutting down')
         self.action_client.destroy()
 
@@ -412,7 +410,7 @@ def shutdown(self):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-    def wait_for_initial_pose(self):
+    def wait_for_initial_pose(self) -> None:
         self.initial_pose_received = False
         while not self.initial_pose_received:
             self.info_msg('Setting initial pose')
@@ -421,7 +419,7 @@ def wait_for_initial_pose(self):
             rclpy.spin_once(self, timeout_sec=1)
 
 
-def test_RobotMovesToGoal(robot_tester):
+def test_RobotMovesToGoal(robot_tester: NavTester) -> bool:
     robot_tester.info_msg('Setting goal pose')
     robot_tester.publishGoalPose()
     robot_tester.info_msg('Waiting 60 seconds for robot to reach goal')
@@ -433,7 +431,7 @@ def test_RobotMovesToGoal(robot_tester):
 # checks that all items in 'limit_passed' permissive array are also true.
 # In other words, it verifies that all speed limits are received
 # exactly (by count and values) as expected by 'limits' array.
-def test_SpeedLimitsAllCorrect(robot_tester):
+def test_SpeedLimitsAllCorrect(robot_tester: NavTester) -> bool:
     if not robot_tester.filter_test_result:
         return False
     for passed in robot_tester.limit_passed:
@@ -443,7 +441,7 @@ def test_SpeedLimitsAllCorrect(robot_tester):
     return True
 
 
-def run_all_tests(robot_tester):
+def run_all_tests(robot_tester: NavTester) -> bool:
     # set transforms to use_sim_time
     result = True
     if result:
@@ -477,7 +475,7 @@ def run_all_tests(robot_tester):
     return result
 
 
-def fwd_pose(x=0.0, y=0.0, z=0.01):
+def fwd_pose(x: float = 0.0, y: float = 0.0, z: float = 0.01) -> Pose:
     initial_pose = Pose()
     initial_pose.position.x = x
     initial_pose.position.y = y
@@ -489,7 +487,7 @@ def fwd_pose(x=0.0, y=0.0, z=0.01):
     return initial_pose
 
 
-def get_tester(args):
+def get_tester(args: argparse.Namespace) -> NavTester:
 
     # Requested tester for one robot
     type_str = args.type
@@ -517,7 +515,7 @@ def get_tester(args):
     return tester
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     # The robot(s) positions from the input arguments
     parser = argparse.ArgumentParser(
         description='System-level costmap filters tester node'
diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py
index 80ff2cd7d10..f46819ef7aa 100755
--- a/nav2_system_tests/src/error_codes/test_error_codes.py
+++ b/nav2_system_tests/src/error_codes/test_error_codes.py
@@ -18,18 +18,13 @@
 import time
 
 from geometry_msgs.msg import PoseStamped
-from nav2_msgs.action import (
-    ComputePathThroughPoses,
-    ComputePathToPose,
-    FollowPath,
-    SmoothPath,
-)
+from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose, FollowPath, SmoothPath
 from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
 from nav_msgs.msg import Path
 import rclpy
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     rclpy.init()
 
     navigator = BasicNavigator()
@@ -118,7 +113,7 @@ def main(argv=sys.argv[1:]):
             result.error_msg != ''
         ), 'Compute path to pose error_msg empty'
 
-    def cancel_task():
+    def cancel_task() -> None:
         time.sleep(1)
         navigator.goal_handle.cancel_goal_async()
 
@@ -179,7 +174,7 @@ def cancel_task():
     a_path.poses.append(pose1)
 
     navigator._waitForNodeToActivate('smoother_server')
-    smoother = {
+    smoother_errors = {
         'invalid_smoother': SmoothPath.Result().INVALID_SMOOTHER,
         'unknown': SmoothPath.Result().UNKNOWN,
         'timeout': SmoothPath.Result().TIMEOUT,
@@ -188,7 +183,7 @@ def cancel_task():
         'invalid_path': SmoothPath.Result().INVALID_PATH,
     }
 
-    for smoother, error_code in smoother.items():
+    for smoother, error_code in smoother_errors.items():
         result = navigator._smoothPathImpl(a_path, smoother)
         assert result.error_code == error_code, 'Smoother error does not match'
         assert result.error_msg != '', 'Smoother error_msg is empty'
diff --git a/nav2_system_tests/src/error_codes/test_error_codes_launch.py b/nav2_system_tests/src/error_codes/test_error_codes_launch.py
index fd8072b43c6..7e33c5bb240 100755
--- a/nav2_system_tests/src/error_codes/test_error_codes_launch.py
+++ b/nav2_system_tests/src/error_codes/test_error_codes_launch.py
@@ -16,16 +16,14 @@
 import os
 import sys
 
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import ExecuteProcess
-from launch.actions import GroupAction
+from launch import LaunchDescription, LaunchService
+from launch.actions import ExecuteProcess, GroupAction
 from launch_ros.actions import Node
 from launch_testing.legacy import LaunchTestService
 
 
-def generate_launch_description():
-    params_file = os.path.join(os.getenv('TEST_DIR'), 'error_code_param.yaml')
+def generate_launch_description() -> LaunchDescription:
+    params_file = os.path.join(os.getenv('TEST_DIR', ''), 'error_code_param.yaml')
 
     remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
 
@@ -105,11 +103,11 @@ def generate_launch_description():
     return ld
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
     test_error_codes_action = ExecuteProcess(
-        cmd=[os.path.join(os.getenv('TEST_DIR'), 'test_error_codes.py')],
+        cmd=[os.path.join(os.getenv('TEST_DIR', ''), 'test_error_codes.py')],
         name='test_error_codes',
         output='screen',
     )
diff --git a/nav2_system_tests/src/gps_navigation/CMakeLists.txt b/nav2_system_tests/src/gps_navigation/CMakeLists.txt
index 742d004f115..5473b38b00e 100644
--- a/nav2_system_tests/src/gps_navigation/CMakeLists.txt
+++ b/nav2_system_tests/src/gps_navigation/CMakeLists.txt
@@ -6,4 +6,3 @@ ament_add_test(test_gps_waypoint_follower
   ENV
     TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
 )
-
diff --git a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py
index 8cc64ec69e1..4705cd77bf2 100644
--- a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py
+++ b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py
@@ -18,7 +18,7 @@
 import launch_ros.actions
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     launch_dir = os.path.dirname(os.path.realpath(__file__))
     params_file = os.path.join(launch_dir, 'dual_ekf_navsat_params.yaml')
     os.environ['FILE_PATH'] = str(launch_dir)
diff --git a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml
index e46ce5d1a8d..c853d75f379 100644
--- a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml
+++ b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml
@@ -124,4 +124,4 @@ navsat_transform:
     broadcast_cartesian_transform: true
     publish_filtered_gps: true
     use_odometry_yaw: true
-    wait_for_datum: false 
+    wait_for_datum: false
diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml
index 3e0042b6638..b81cef7ce56 100644
--- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml
+++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml
@@ -61,6 +61,7 @@ controller_server:
       ax_max: 3.0
       ax_min: -3.0
       ay_max: 3.0
+      ay_min: -3.0
       az_max: 3.5
       vx_std: 0.2
       vy_std: 0.2
@@ -82,10 +83,17 @@ controller_server:
         time_step: 3
       AckermannConstraints:
         min_turning_r: 0.2
-      critics: [
-        "ConstraintCritic", "CostCritic", "GoalCritic",
-        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
-        "PathAngleCritic", "PreferForwardCritic"]
+      critics:
+        [
+          "ConstraintCritic",
+          "CostCritic",
+          "GoalCritic",
+          "GoalAngleCritic",
+          "PathAlignCritic",
+          "PathFollowCritic",
+          "PathAngleCritic",
+          "PreferForwardCritic",
+        ]
       ConstraintCritic:
         enabled: true
         cost_power: 1
@@ -109,8 +117,9 @@ controller_server:
         enabled: true
         cost_power: 1
         cost_weight: 3.81
+        near_collision_cost: 253
         critical_cost: 300.0
-        consider_footprint: true
+        consider_footprint: false
         collision_cost: 1000000.0
         near_goal_distance: 1.0
         trajectory_point_step: 2
@@ -194,7 +203,7 @@ global_costmap:
       robot_radius: 0.22
       resolution: 0.1
       # When using GPS navigation you will potentially traverse huge environments which are not practical to
-      # fit on a big static costmap. Thus it is recommended to use a rolling global costmap large enough to 
+      # fit on a big static costmap. Thus it is recommended to use a rolling global costmap large enough to
       # contain each pair of successive waypoints. See: https://github.com/ros-planning/navigation2/issues/2174
       rolling_window: True
       width: 50
@@ -266,7 +275,8 @@ behavior_server:
     local_footprint_topic: local_costmap/published_footprint
     global_footprint_topic: global_costmap/published_footprint
     cycle_frequency: 10.0
-    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
+    behavior_plugins:
+      ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
     spin:
       plugin: "nav2_behaviors::Spin"
     backup:
@@ -352,18 +362,18 @@ docking_server:
     max_retries: 3
     base_frame: "base_link"
     fixed_frame: "odom"
-    dock_backwards: false
     dock_prestaging_tolerance: 0.5
 
     # Types of docks
-    dock_plugins: ['simple_charging_dock']
+    dock_plugins: ["simple_charging_dock"]
     simple_charging_dock:
-      plugin: 'opennav_docking::SimpleChargingDock'
+      plugin: "opennav_docking::SimpleChargingDock"
       docking_threshold: 0.05
       staging_x_offset: -0.7
       use_external_detection_pose: true
       use_battery_status: false # true
       use_stall_detection: false # true
+      dock_direction: "forward"
 
       external_detection_timeout: 1.0
       external_detection_translation_x: -0.18
@@ -374,9 +384,9 @@ docking_server:
       filter_coef: 0.1
 
     # Dock instances
-    docks: ['home_dock']  # Input your docks here
+    docks: ["home_dock"] # Input your docks here
     home_dock:
-      type: 'simple_charging_dock'
+      type: "simple_charging_dock"
       frame: map
       pose: [0.0, 0.0, 0.0]
 
@@ -384,4 +394,4 @@ docking_server:
       k_phi: 3.0
       k_delta: 2.0
       v_linear_min: 0.15
-      v_linear_max: 0.15
\ No newline at end of file
+      v_linear_max: 0.15
diff --git a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py
index cb11b2c36d7..140be5ab708 100755
--- a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py
+++ b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py
@@ -18,23 +18,16 @@
 import sys
 
 from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import (
-    AppendEnvironmentVariable,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    SetEnvironmentVariable,
-)
+from launch import LaunchDescription, LaunchService
+from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription,
+                            SetEnvironmentVariable)
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_testing.legacy import LaunchTestService
-
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
 
@@ -117,11 +110,11 @@ def generate_launch_description():
     )
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
     test1_action = ExecuteProcess(
-        cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester.py')],
+        cmd=[os.path.join(os.getenv('TEST_DIR', ''), 'tester.py')],
         name='tester_node',
         output='screen',
     )
diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py
index 46572469c98..56c99324b29 100755
--- a/nav2_system_tests/src/gps_navigation/tester.py
+++ b/nav2_system_tests/src/gps_navigation/tester.py
@@ -15,36 +15,38 @@
 
 import sys
 import time
+from typing import Optional
 
 from action_msgs.msg import GoalStatus
 from geographic_msgs.msg import GeoPose
 from nav2_msgs.action import ComputePathToPose, FollowGPSWaypoints
 from nav2_msgs.srv import ManageLifecycleNodes
 from rcl_interfaces.srv import SetParameters
-
 import rclpy
-
-from rclpy.action import ActionClient
+from rclpy.action import ActionClient  # type: ignore[attr-defined]
+from rclpy.action.client import ClientGoalHandle
 from rclpy.node import Node
 from rclpy.parameter import Parameter
 
 
 class GpsWaypointFollowerTest(Node):
 
-    def __init__(self):
+    def __init__(self) -> None:
         super().__init__(node_name='nav2_gps_waypoint_tester', namespace='')
-        self.waypoints = None
+        self.waypoints: list[float] = []
         self.action_client = ActionClient(
             self, FollowGPSWaypoints, 'follow_gps_waypoints'
         )
-        self.goal_handle = None
-        self.action_result = None
+        self.goal_handle: Optional[ClientGoalHandle[
+                FollowGPSWaypoints.Goal, FollowGPSWaypoints.Result,
+                FollowGPSWaypoints.Feedback]] = None
+        self.action_result = FollowGPSWaypoints.Result()
 
         self.param_cli = self.create_client(
             SetParameters, '/waypoint_follower/set_parameters'
         )
 
-    def setWaypoints(self, waypoints):
+    def setWaypoints(self, waypoints: list[list[float]]) -> None:
         self.waypoints = []
         for wp in waypoints:
             msg = GeoPose()
@@ -53,7 +55,7 @@ def setWaypoints(self, waypoints):
             msg.orientation.w = 1.0
             self.waypoints.append(msg)
 
-    def run(self, block, cancel):
+    def run(self, block: bool, cancel: bool) -> bool:
         # if not self.waypoints:
         #     rclpy.error_msg('Did not set valid waypoints before running test!')
         #     return False
@@ -74,7 +76,7 @@ def run(self, block, cancel):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -90,8 +92,8 @@ def run(self, block, cancel):
         self.info_msg("Waiting for 'follow_gps_waypoints' action to complete")
         try:
             rclpy.spin_until_future_complete(self, get_result_future)
-            status = get_result_future.result().status
-            result = get_result_future.result().result
+            status = get_result_future.result().status  # type: ignore[union-attr]
+            result = get_result_future.result().result  # type: ignore[union-attr]
             self.action_result = result
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
@@ -102,14 +104,14 @@ def run(self, block, cancel):
         if len(result.missed_waypoints) > 0:
             self.info_msg(
                 'Goal failed to process all waypoints,'
-                ' missed {0} wps.'.format(len(result.missed_waypoints))
+                f' missed {len(result.missed_waypoints)} wps.'
             )
             return False
 
         self.info_msg('Goal succeeded!')
         return True
 
-    def setStopFailureParam(self, value):
+    def setStopFailureParam(self, value: bool) -> None:
         req = SetParameters.Request()
         req.parameters = [
             Parameter('stop_on_failure', Parameter.Type.BOOL, value).to_parameter_msg()
@@ -117,7 +119,7 @@ def setStopFailureParam(self, value):
         future = self.param_cli.call_async(req)
         rclpy.spin_until_future_complete(self, future)
 
-    def shutdown(self):
+    def shutdown(self) -> None:
         self.info_msg('Shutting down')
 
         self.action_client.destroy()
@@ -139,21 +141,21 @@ def shutdown(self):
 
         self.info_msg(f'{transition_service} finished')
 
-    def cancel_goal(self):
-        cancel_future = self.goal_handle.cancel_goal_async()
+    def cancel_goal(self) -> None:
+        cancel_future = self.goal_handle.cancel_goal_async()  # type: ignore[union-attr]
         rclpy.spin_until_future_complete(self, cancel_future)
 
-    def info_msg(self, msg: str):
+    def info_msg(self, msg: str) -> None:
         self.get_logger().info(msg)
 
-    def warn_msg(self, msg: str):
+    def warn_msg(self, msg: str) -> None:
         self.get_logger().warn(msg)
 
-    def error_msg(self, msg: str):
+    def error_msg(self, msg: str) -> None:
         self.get_logger().error(msg)
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     rclpy.init()
 
     # wait a few seconds to make sure entire stacks are up
diff --git a/nav2_system_tests/src/localization/README.md b/nav2_system_tests/src/localization/README.md
index a5e20c395f6..1bd74e9957c 100644
--- a/nav2_system_tests/src/localization/README.md
+++ b/nav2_system_tests/src/localization/README.md
@@ -9,7 +9,7 @@ First, build the package
 ```
 colcon build --symlink-install
 ```
-After building, from /build/nav2_system_tests directory run: 
+After building, from /build/nav2_system_tests directory run:
 ```
 ctest -V -R test_localization
 ```
diff --git a/nav2_system_tests/src/localization/test_localization_launch.py b/nav2_system_tests/src/localization/test_localization_launch.py
index 619b989510d..c573f39dd56 100755
--- a/nav2_system_tests/src/localization/test_localization_launch.py
+++ b/nav2_system_tests/src/localization/test_localization_launch.py
@@ -19,17 +19,15 @@
 import sys
 
 from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import AppendEnvironmentVariable, ExecuteProcess
-from launch.actions import IncludeLaunchDescription
+from launch import LaunchDescription, LaunchService
+from launch.actions import AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 import launch_ros.actions
 from launch_testing.legacy import LaunchTestService
 
 
-def main(argv=sys.argv[1:]):
-    testExecutable = os.getenv('TEST_EXECUTABLE')
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
+    testExecutable = os.getenv('TEST_EXECUTABLE', '')
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
 
diff --git a/nav2_system_tests/src/localization/test_localization_node.cpp b/nav2_system_tests/src/localization/test_localization_node.cpp
index 794abe33a8c..6f9c2daa449 100644
--- a/nav2_system_tests/src/localization/test_localization_node.cpp
+++ b/nav2_system_tests/src/localization/test_localization_node.cpp
@@ -110,7 +110,7 @@ TEST_F(TestAmclPose, SimpleAmclTest)
   EXPECT_EQ(true, defaultAmclTest());
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_system_tests/src/planning/README.md b/nav2_system_tests/src/planning/README.md
index 6e95d215ba3..0dcfb157d13 100644
--- a/nav2_system_tests/src/planning/README.md
+++ b/nav2_system_tests/src/planning/README.md
@@ -12,4 +12,4 @@ Below is an example of the output from randomized testing. Blue spheres represen
 
 *Note: Currently robot size is 1x1 cells, no obstacle inflation is done on the costmap*
 
-*Note: The Navfn algorithm sometimes fails to generate a path as you can see from the 'orphan' spheres.*
\ No newline at end of file
+*Note: The Navfn algorithm sometimes fails to generate a path as you can see from the 'orphan' spheres.*
diff --git a/nav2_system_tests/src/planning/test_planner_costmaps_launch.py b/nav2_system_tests/src/planning/test_planner_costmaps_launch.py
index 09bd3d34eee..6eb5888e80f 100755
--- a/nav2_system_tests/src/planning/test_planner_costmaps_launch.py
+++ b/nav2_system_tests/src/planning/test_planner_costmaps_launch.py
@@ -17,14 +17,12 @@
 import os
 import sys
 
-from launch import LaunchDescription
-from launch import LaunchService
+from launch import LaunchDescription, LaunchService
 from launch.actions import ExecuteProcess
-
 from launch_testing.legacy import LaunchTestService
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     testExecutable = os.getenv('TEST_EXECUTABLE')
 
     ld = LaunchDescription([])
diff --git a/nav2_system_tests/src/planning/test_planner_random_launch.py b/nav2_system_tests/src/planning/test_planner_random_launch.py
index a7747e513ab..ca014d6a59f 100755
--- a/nav2_system_tests/src/planning/test_planner_random_launch.py
+++ b/nav2_system_tests/src/planning/test_planner_random_launch.py
@@ -17,14 +17,12 @@
 import os
 import sys
 
-from launch import LaunchDescription
-from launch import LaunchService
+from launch import LaunchDescription, LaunchService
 from launch.actions import ExecuteProcess
-
 from launch_testing.legacy import LaunchTestService
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     testExecutable = os.getenv('TEST_EXECUTABLE')
 
     ld = LaunchDescription([])
diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt
index 42b433f64a5..3db27c6398e 100644
--- a/nav2_system_tests/src/system/CMakeLists.txt
+++ b/nav2_system_tests/src/system/CMakeLists.txt
@@ -4,12 +4,12 @@ ament_add_test(test_bt_navigator
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   TIMEOUT 180
   ENV
-    TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
-    TESTER=nav_to_pose_tester_node.py
-    ASTAR=True
-    CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
-    PLANNER=nav2_navfn_planner::NavfnPlanner
+  TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
+  BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+  TESTER=nav_to_pose_tester_node.py
+  ASTAR=True
+  CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
+  PLANNER=nav2_navfn_planner::NavfnPlanner
 )
 
 ament_add_test(test_bt_navigator_with_wrong_init_pose
@@ -18,12 +18,12 @@ ament_add_test(test_bt_navigator_with_wrong_init_pose
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   TIMEOUT 180
   ENV
-    TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
-    TESTER=nav_to_pose_tester_node.py
-    ASTAR=True
-    CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
-    PLANNER=nav2_navfn_planner::NavfnPlanner
+  TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
+  BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+  TESTER=nav_to_pose_tester_node.py
+  ASTAR=True
+  CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
+  PLANNER=nav2_navfn_planner::NavfnPlanner
 )
 
 ament_add_test(test_bt_navigator_with_dijkstra
@@ -32,26 +32,27 @@ ament_add_test(test_bt_navigator_with_dijkstra
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   TIMEOUT 180
   ENV
-    TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
-    TESTER=nav_to_pose_tester_node.py
-    ASTAR=False
-    CONTROLLER=dwb_core::DWBLocalPlanner
-    PLANNER=nav2_navfn_planner::NavfnPlanner
+  TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
+  BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+  TESTER=nav_to_pose_tester_node.py
+  ASTAR=False
+  CONTROLLER=dwb_core::DWBLocalPlanner
+  PLANNER=nav2_navfn_planner::NavfnPlanner
 )
 
-ament_add_test(test_bt_navigator_2
+ament_add_test(test_bt_navigator_with_groot_monitoring
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   TIMEOUT 180
   ENV
-    TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
-    TESTER=nav_to_pose_tester_node.py
-    ASTAR=False
-    CONTROLLER=dwb_core::DWBLocalPlanner
-    PLANNER=nav2_navfn_planner::NavfnPlanner
+  TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
+  BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+  TESTER=nav_to_pose_tester_node.py
+  ASTAR=False
+  GROOT_MONITORING=True
+  CONTROLLER=dwb_core::DWBLocalPlanner
+  PLANNER=nav2_navfn_planner::NavfnPlanner
 )
 
 ament_add_test(test_dynamic_obstacle
@@ -60,13 +61,13 @@ ament_add_test(test_dynamic_obstacle
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   TIMEOUT 180
   ENV
-    TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    ADD_OBSTACLE=True
-    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
-    TESTER=nav_to_pose_tester_node.py
-    ASTAR=False
-    CONTROLLER=dwb_core::DWBLocalPlanner
-    PLANNER=nav2_navfn_planner::NavfnPlanner
+  TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
+  ADD_OBSTACLE=True
+  BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+  TESTER=nav_to_pose_tester_node.py
+  ASTAR=False
+  CONTROLLER=dwb_core::DWBLocalPlanner
+  PLANNER=nav2_navfn_planner::NavfnPlanner
 )
 
 ament_add_test(test_nav_through_poses
@@ -75,13 +76,13 @@ ament_add_test(test_nav_through_poses
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   TIMEOUT 180
   ENV
-    TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    ADD_OBSTACLE=True
-    BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml
-    TESTER=nav_through_poses_tester_node.py
-    ASTAR=False
-    CONTROLLER=dwb_core::DWBLocalPlanner
-    PLANNER=nav2_navfn_planner::NavfnPlanner
+  TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
+  ADD_OBSTACLE=True
+  BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml
+  TESTER=nav_through_poses_tester_node.py
+  ASTAR=False
+  CONTROLLER=dwb_core::DWBLocalPlanner
+  PLANNER=nav2_navfn_planner::NavfnPlanner
 )
 
 ament_add_test(test_nav_through_poses_error_msg
@@ -90,28 +91,28 @@ ament_add_test(test_nav_through_poses_error_msg
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   TIMEOUT 180
   ENV
-    TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    ADD_OBSTACLE=True
-    BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml
-    TESTER=nav_through_poses_tester_error_msg_node.py
-    ASTAR=False
-    CONTROLLER=dwb_core::DWBLocalPlanner
-    PLANNER=nav2_navfn_planner::NavfnPlanner
+  TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
+  ADD_OBSTACLE=True
+  BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml
+  TESTER=nav_through_poses_tester_error_msg_node.py
+  ASTAR=False
+  CONTROLLER=dwb_core::DWBLocalPlanner
+  PLANNER=nav2_navfn_planner::NavfnPlanner
 )
 
 # ament_add_test(test_multi_robot
-#   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-#   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_multi_robot_launch.py"
-#   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
-#   TIMEOUT 180
-#   ENV
-#     TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-#     TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
-#     TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/world_only.model
-#     TEST_URDF=${PROJECT_SOURCE_DIR}/urdf/turtlebot3_waffle.urdf
-#     TEST_SDF=${PROJECT_SOURCE_DIR}/models/turtlebot3_waffle/model.sdf
-#     BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
-#     CONTROLLER=dwb_core::DWBLocalPlanner
-#     PLANNER=nav2_navfn_planner::NavfnPlanner
-#     TESTER=nav_to_pose_tester_node.py
+# GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_multi_robot_launch.py"
+# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+# TIMEOUT 180
+# ENV
+# TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
+# TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
+# TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/world_only.model
+# TEST_URDF=${PROJECT_SOURCE_DIR}/urdf/turtlebot3_waffle.urdf
+# TEST_SDF=${PROJECT_SOURCE_DIR}/models/turtlebot3_waffle/model.sdf
+# BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+# CONTROLLER=dwb_core::DWBLocalPlanner
+# PLANNER=nav2_navfn_planner::NavfnPlanner
+# TESTER=nav_to_pose_tester_node.py
 # )
diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml
index 9d71b2912a5..311081aaa26 100644
--- a/nav2_system_tests/src/system/nav2_system_params.yaml
+++ b/nav2_system_tests/src/system/nav2_system_params.yaml
@@ -50,6 +50,8 @@ bt_navigator:
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
+      enable_groot_monitoring: true
+      groot_server_port: 1667
     navigate_through_poses:
       plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
     error_code_name_prefixes:
@@ -125,7 +127,16 @@ controller_server:
       trans_stopped_velocity: 0.25
       short_circuit_trajectory_evaluation: True
       stateful: True
-      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      critics:
+        [
+          "RotateToGoal",
+          "Oscillation",
+          "BaseObstacle",
+          "GoalAlign",
+          "PathAlign",
+          "PathDist",
+          "GoalDist",
+        ]
       BaseObstacle.scale: 0.02
       PathAlign.scale: 32.0
       PathAlign.forward_point_distance: 0.1
@@ -255,7 +266,8 @@ behavior_server:
     local_footprint_topic: local_costmap/published_footprint
     global_footprint_topic: global_costmap/published_footprint
     cycle_frequency: 10.0
-    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
+    behavior_plugins:
+      ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
     spin:
       plugin: "nav2_behaviors::Spin"
     backup:
@@ -342,18 +354,18 @@ docking_server:
     max_retries: 3
     base_frame: "base_link"
     fixed_frame: "odom"
-    dock_backwards: false
     dock_prestaging_tolerance: 0.5
 
     # Types of docks
-    dock_plugins: ['simple_charging_dock']
+    dock_plugins: ["simple_charging_dock"]
     simple_charging_dock:
-      plugin: 'opennav_docking::SimpleChargingDock'
+      plugin: "opennav_docking::SimpleChargingDock"
       docking_threshold: 0.05
       staging_x_offset: -0.7
       use_external_detection_pose: true
       use_battery_status: false # true
       use_stall_detection: false # true
+      dock_direction: "forward"
 
       external_detection_timeout: 1.0
       external_detection_translation_x: -0.18
@@ -364,9 +376,9 @@ docking_server:
       filter_coef: 0.1
 
     # Dock instances
-    docks: ['home_dock']  # Input your docks here
+    docks: ["home_dock"] # Input your docks here
     home_dock:
-      type: 'simple_charging_dock'
+      type: "simple_charging_dock"
       frame: map
       pose: [0.0, 0.0, 0.0]
 
@@ -375,4 +387,3 @@ docking_server:
       k_delta: 2.0
       v_linear_min: 0.15
       v_linear_max: 0.15
-
diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py
index 7ff35af2aa8..c170d91338a 100755
--- a/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py
+++ b/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py
@@ -17,26 +17,19 @@
 import argparse
 import sys
 import time
-
 from typing import Optional
 
 from action_msgs.msg import GoalStatus
-from geometry_msgs.msg import Pose
-from geometry_msgs.msg import PoseStamped
-from geometry_msgs.msg import PoseWithCovarianceStamped
+from geometry_msgs.msg import Pose, PoseStamped, PoseWithCovarianceStamped
 from lifecycle_msgs.srv import GetState
 from nav2_msgs.action import NavigateThroughPoses
 from nav2_msgs.srv import ManageLifecycleNodes
 from rcl_interfaces.srv import SetParameters
-
 import rclpy
-
 from rclpy.action.client import ActionClient
 from rclpy.node import Node
 from rclpy.parameter import Parameter
-from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
-from rclpy.qos import QoSProfile
-
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
 from std_msgs.msg import String
 
 
@@ -82,16 +75,16 @@ def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''):
             SetParameters, '/controller_server/set_parameters'
         )
 
-    def info_msg(self, msg: str):
+    def info_msg(self, msg: str) -> None:
         self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
 
-    def warn_msg(self, msg: str):
+    def warn_msg(self, msg: str) -> None:
         self.get_logger().warning('\033[1;37;43m' + msg + '\033[0m')
 
-    def error_msg(self, msg: str):
+    def error_msg(self, msg: str) -> None:
         self.get_logger().error('\033[1;37;41m' + msg + '\033[0m')
 
-    def setInitialPose(self):
+    def setInitialPose(self) -> None:
         msg = PoseWithCovarianceStamped()
         msg.pose.pose = self.initial_pose
         msg.header.frame_id = 'map'
@@ -99,17 +92,17 @@ def setInitialPose(self):
         self.initial_pose_pub.publish(msg)
         self.currentPose = self.initial_pose
 
-    def setGoalChecker(self, name):
+    def setGoalChecker(self, name: str) -> None:
         msg = String()
         msg.data = name
         self.goal_checker_selector_pub.publish(msg)
 
-    def setProgressChecker(self, name):
+    def setProgressChecker(self, name: str) -> None:
         msg = String()
         msg.data = name
         self.progress_checker_selector_pub.publish(msg)
 
-    def setControllerParam(self, name, parameter_type, value):
+    def setControllerParam(self, name: str, parameter_type: Parameter.Type, value: float) -> None:
         req = SetParameters.Request()
         req.parameters = [
             Parameter(name, parameter_type, value).to_parameter_msg()
@@ -117,7 +110,7 @@ def setControllerParam(self, name, parameter_type, value):
         future = self.controller_param_cli.call_async(req)
         rclpy.spin_until_future_complete(self, future)
 
-    def getStampedPoseMsg(self, pose: Pose):
+    def getStampedPoseMsg(self, pose: Pose) -> PoseStamped:
         msg = PoseStamped()
         msg.header.frame_id = 'map'
         msg.pose = pose
@@ -127,7 +120,7 @@ def runNavigateAction(self,
                           goal_pose: Optional[Pose] = None,
                           behavior_tree: Optional[str] = None,
                           expected_error_code: Optional[int] = None,
-                          expected_error_msg: Optional[str] = None):
+                          expected_error_msg: Optional[str] = None) -> bool:
         # Sends a `NavToPose` action request and waits for completion
         self.info_msg("Waiting for 'NavigateThroughPoses' action server")
         while not self.action_client.wait_for_server(timeout_sec=1.0):
@@ -160,9 +153,9 @@ def runNavigateAction(self,
 
         self.info_msg("Waiting for 'NavigateToPose' action to complete")
         rclpy.spin_until_future_complete(self, get_result_future)
-        status = get_result_future.result().status
+        status = get_result_future.result().status  # type: ignore[union-attr]
         if status != GoalStatus.STATUS_SUCCEEDED:
-            result = get_result_future.result().result
+            result = get_result_future.result().result  # type: ignore[union-attr]
             if (result.error_code == expected_error_code and
                result.error_msg == expected_error_msg):
                 self.info_msg(f'Goal failed as expected with status code: {status}'
@@ -180,12 +173,12 @@ def runNavigateAction(self,
         self.info_msg('Goal succeeded!')
         return True
 
-    def poseCallback(self, msg):
+    def poseCallback(self, msg: PoseWithCovarianceStamped) -> None:
         self.info_msg('Received amcl_pose')
         self.current_pose = msg.pose.pose
         self.initial_pose_received = True
 
-    def wait_for_node_active(self, node_name: str):
+    def wait_for_node_active(self, node_name: str) -> None:
         # Waits for the node within the tester namespace to become active
         self.info_msg(f'Waiting for {node_name} to become active')
         node_service = f'{node_name}/get_state'
@@ -199,7 +192,7 @@ def wait_for_node_active(self, node_name: str):
             future = state_client.call_async(req)
             rclpy.spin_until_future_complete(self, future)
             if future.result() is not None:
-                state = future.result().current_state.label
+                state = future.result().current_state.label  # type: ignore[union-attr]
                 self.info_msg(f'Result of get_state: {state}')
             else:
                 self.error_msg(
@@ -207,7 +200,7 @@ def wait_for_node_active(self, node_name: str):
                 )
             time.sleep(5)
 
-    def shutdown(self):
+    def shutdown(self) -> None:
         self.info_msg('Shutting down')
         self.action_client.destroy()
 
@@ -242,7 +235,7 @@ def shutdown(self):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-    def wait_for_initial_pose(self):
+    def wait_for_initial_pose(self) -> None:
         self.initial_pose_received = False
         while not self.initial_pose_received:
             self.info_msg('Setting initial pose')
@@ -251,7 +244,7 @@ def wait_for_initial_pose(self):
             rclpy.spin_once(self, timeout_sec=1)
 
 
-def run_all_tests(robot_tester):
+def run_all_tests(robot_tester: NavTester) -> bool:
     pose_out_of_bounds = Pose()
     pose_out_of_bounds.position.x = 2000.0
     pose_out_of_bounds.position.y = 4000.0
@@ -348,7 +341,7 @@ def run_all_tests(robot_tester):
     return result
 
 
-def fwd_pose(x=0.0, y=0.0, z=0.01):
+def fwd_pose(x: float = 0.0, y: float = 0.0, z: float = 0.01) -> Pose:
     initial_pose = Pose()
     initial_pose.position.x = x
     initial_pose.position.y = y
@@ -360,7 +353,7 @@ def fwd_pose(x=0.0, y=0.0, z=0.01):
     return initial_pose
 
 
-def get_testers(args):
+def get_testers(args: argparse.Namespace) -> list[NavTester]:
     testers = []
 
     init_x, init_y, final_x, final_y = args.robot[0]
@@ -383,7 +376,7 @@ def get_testers(args):
     return testers
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     # The robot(s) positions from the input arguments
     parser = argparse.ArgumentParser(description='System-level navigation tester node')
     group = parser.add_mutually_exclusive_group(required=True)
diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_node.py
index d3a2a8251f4..02c793d5507 100755
--- a/nav2_system_tests/src/system/nav_through_poses_tester_node.py
+++ b/nav2_system_tests/src/system/nav_through_poses_tester_node.py
@@ -17,23 +17,17 @@
 import argparse
 import sys
 import time
-
 from typing import Optional
 
 from action_msgs.msg import GoalStatus
-from geometry_msgs.msg import Pose
-from geometry_msgs.msg import PoseStamped
-from geometry_msgs.msg import PoseWithCovarianceStamped
+from geometry_msgs.msg import Pose, PoseStamped, PoseWithCovarianceStamped
 from lifecycle_msgs.srv import GetState
 from nav2_msgs.action import NavigateThroughPoses
 from nav2_msgs.srv import ManageLifecycleNodes
-
 import rclpy
-
-from rclpy.action import ActionClient
+from rclpy.action import ActionClient  # type: ignore[attr-defined]
 from rclpy.node import Node
-from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
-from rclpy.qos import QoSProfile
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
 
 
 class NavTester(Node):
@@ -61,16 +55,16 @@ def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''):
             self, NavigateThroughPoses, 'navigate_through_poses'
         )
 
-    def info_msg(self, msg: str):
+    def info_msg(self, msg: str) -> None:
         self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
 
-    def warn_msg(self, msg: str):
+    def warn_msg(self, msg: str) -> None:
         self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m')
 
-    def error_msg(self, msg: str):
+    def error_msg(self, msg: str) -> None:
         self.get_logger().error('\033[1;37;41m' + msg + '\033[0m')
 
-    def setInitialPose(self):
+    def setInitialPose(self) -> None:
         msg = PoseWithCovarianceStamped()
         msg.pose.pose = self.initial_pose
         msg.header.frame_id = 'map'
@@ -78,13 +72,13 @@ def setInitialPose(self):
         self.initial_pose_pub.publish(msg)
         self.currentPose = self.initial_pose
 
-    def getStampedPoseMsg(self, pose: Pose):
+    def getStampedPoseMsg(self, pose: Pose) -> PoseStamped:
         msg = PoseStamped()
         msg.header.frame_id = 'map'
         msg.pose = pose
         return msg
 
-    def runNavigateAction(self, goal_pose: Optional[Pose] = None):
+    def runNavigateAction(self, goal_pose: Optional[Pose] = None) -> bool:
         # Sends a `NavToPose` action request and waits for completion
         self.info_msg("Waiting for 'NavigateThroughPoses' action server")
         while not self.action_client.wait_for_server(timeout_sec=1.0):
@@ -107,7 +101,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
         rclpy.spin_until_future_complete(self, send_goal_future)
         goal_handle = send_goal_future.result()
 
-        if not goal_handle.accepted:
+        if not goal_handle or not goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -116,9 +110,9 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
 
         self.info_msg("Waiting for 'NavigateToPose' action to complete")
         rclpy.spin_until_future_complete(self, get_result_future)
-        status = get_result_future.result().status
+        status = get_result_future.result().status  # type: ignore[union-attr]
         if status != GoalStatus.STATUS_SUCCEEDED:
-            result = get_result_future.result().result
+            result = get_result_future.result().result  # type: ignore[union-attr]
             self.info_msg(f'Goal failed with status code: {status}'
                           f' error code:{result.error_code}'
                           f' error msg:{result.error_msg}')
@@ -127,7 +121,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
         self.info_msg('Goal succeeded!')
         return True
 
-    def runFakeNavigateAction(self):
+    def runFakeNavigateAction(self) -> bool:
         # Sends a `NavToPose` action request and waits for completion
         self.info_msg("Waiting for 'NavigateThroughPoses' action server")
         while not self.action_client.wait_for_server(timeout_sec=1.0):
@@ -143,7 +137,7 @@ def runFakeNavigateAction(self):
         rclpy.spin_until_future_complete(self, send_goal_future)
         goal_handle = send_goal_future.result()
 
-        if not goal_handle.accepted:
+        if not goal_handle or not goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -152,9 +146,9 @@ def runFakeNavigateAction(self):
 
         self.info_msg("Waiting for 'NavigateToPose' action to complete")
         rclpy.spin_until_future_complete(self, get_result_future)
-        status = get_result_future.result().status
+        status = get_result_future.result().status  # type: ignore[union-attr]
         if status != GoalStatus.STATUS_SUCCEEDED:
-            result = get_result_future.result().result
+            result = get_result_future.result().result  # type: ignore[union-attr]
             self.info_msg(f'Goal failed with status code: {status}'
                           f' error code:{result.error_code}'
                           f' error msg:{result.error_msg}')
@@ -163,7 +157,7 @@ def runFakeNavigateAction(self):
         self.info_msg('Goal succeeded!')
         return True
 
-    def runNavigatePreemptionAction(self, block):
+    def runNavigatePreemptionAction(self, block: bool) -> bool:
         # Sends a `NavToPose` action request and waits for completion
         self.info_msg("Waiting for 'NavigateThroughPoses' action server")
         while not self.action_client.wait_for_server(timeout_sec=1.0):
@@ -180,7 +174,7 @@ def runNavigatePreemptionAction(self, block):
         rclpy.spin_until_future_complete(self, send_goal_future)
         goal_handle = send_goal_future.result()
 
-        if not goal_handle.accepted:
+        if not goal_handle or not goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -192,9 +186,9 @@ def runNavigatePreemptionAction(self, block):
 
         self.info_msg("Waiting for 'NavigateToPose' action to complete")
         rclpy.spin_until_future_complete(self, get_result_future)
-        status = get_result_future.result().status
+        status = get_result_future.result().status  # type: ignore[union-attr]
         if status != GoalStatus.STATUS_SUCCEEDED:
-            result = get_result_future.result().result
+            result = get_result_future.result().result  # type: ignore[union-attr]
             self.info_msg(f'Goal failed with status code: {status}'
                           f' error code:{result.error_code}'
                           f' error msg:{result.error_msg}')
@@ -203,12 +197,12 @@ def runNavigatePreemptionAction(self, block):
         self.info_msg('Goal succeeded!')
         return True
 
-    def poseCallback(self, msg):
+    def poseCallback(self, msg: PoseWithCovarianceStamped) -> None:
         self.info_msg('Received amcl_pose')
         self.current_pose = msg.pose.pose
         self.initial_pose_received = True
 
-    def wait_for_node_active(self, node_name: str):
+    def wait_for_node_active(self, node_name: str) -> None:
         # Waits for the node within the tester namespace to become active
         self.info_msg(f'Waiting for {node_name} to become active')
         node_service = f'{node_name}/get_state'
@@ -222,7 +216,7 @@ def wait_for_node_active(self, node_name: str):
             future = state_client.call_async(req)
             rclpy.spin_until_future_complete(self, future)
             if future.result() is not None:
-                state = future.result().current_state.label
+                state = future.result().current_state.label  # type: ignore[union-attr]
                 self.info_msg(f'Result of get_state: {state}')
             else:
                 self.error_msg(
@@ -230,7 +224,7 @@ def wait_for_node_active(self, node_name: str):
                 )
             time.sleep(5)
 
-    def shutdown(self):
+    def shutdown(self) -> None:
         self.info_msg('Shutting down')
         self.action_client.destroy()
 
@@ -265,7 +259,7 @@ def shutdown(self):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-    def wait_for_initial_pose(self):
+    def wait_for_initial_pose(self) -> None:
         self.initial_pose_received = False
         while not self.initial_pose_received:
             self.info_msg('Setting initial pose')
@@ -274,7 +268,7 @@ def wait_for_initial_pose(self):
             rclpy.spin_once(self, timeout_sec=1)
 
 
-def run_all_tests(robot_tester):
+def run_all_tests(robot_tester: NavTester) -> bool:
     # set transforms to use_sim_time
     result = True
     if result:
@@ -298,7 +292,7 @@ def run_all_tests(robot_tester):
     return result
 
 
-def fwd_pose(x=0.0, y=0.0, z=0.01):
+def fwd_pose(x: float = 0.0, y: float = 0.0, z: float = 0.01) -> Pose:
     initial_pose = Pose()
     initial_pose.position.x = x
     initial_pose.position.y = y
@@ -310,7 +304,7 @@ def fwd_pose(x=0.0, y=0.0, z=0.01):
     return initial_pose
 
 
-def get_testers(args):
+def get_testers(args: argparse.Namespace) -> list[NavTester]:
     testers = []
 
     init_x, init_y, final_x, final_y = args.robot[0]
@@ -333,7 +327,7 @@ def get_testers(args):
     return testers
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     # The robot(s) positions from the input arguments
     parser = argparse.ArgumentParser(description='System-level navigation tester node')
     group = parser.add_mutually_exclusive_group(required=True)
diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py
index 0b2c777b860..222d0f8237a 100755
--- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py
+++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py
@@ -15,26 +15,24 @@
 # limitations under the License.
 
 import argparse
+import json
 import math
+import os
+import struct
 import sys
 import time
-
-from typing import Optional
+from typing import Any, Optional
 
 from action_msgs.msg import GoalStatus
-from geometry_msgs.msg import Pose
-from geometry_msgs.msg import PoseStamped
-from geometry_msgs.msg import PoseWithCovarianceStamped
+from geometry_msgs.msg import Pose, PoseStamped, PoseWithCovarianceStamped
 from lifecycle_msgs.srv import GetState
 from nav2_msgs.action import NavigateToPose
 from nav2_msgs.srv import ManageLifecycleNodes
-
 import rclpy
-
-from rclpy.action import ActionClient
+from rclpy.action import ActionClient  # type: ignore[attr-defined]
 from rclpy.node import Node
-from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
-from rclpy.qos import QoSProfile
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
+import zmq
 
 
 class NavTester(Node):
@@ -62,16 +60,16 @@ def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''):
         self.set_initial_pose_timeout = 15
         self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
 
-    def info_msg(self, msg: str):
+    def info_msg(self, msg: str) -> None:
         self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
 
-    def warn_msg(self, msg: str):
+    def warn_msg(self, msg: str) -> None:
         self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m')
 
-    def error_msg(self, msg: str):
+    def error_msg(self, msg: str) -> None:
         self.get_logger().error('\033[1;37;41m' + msg + '\033[0m')
 
-    def setInitialPose(self):
+    def setInitialPose(self) -> None:
         msg = PoseWithCovarianceStamped()
         msg.pose.pose = self.initial_pose
         msg.header.frame_id = 'map'
@@ -79,22 +77,28 @@ def setInitialPose(self):
         self.initial_pose_pub.publish(msg)
         self.currentPose = self.initial_pose
 
-    def getStampedPoseMsg(self, pose: Pose):
+    def getStampedPoseMsg(self, pose: Pose) -> PoseStamped:
         msg = PoseStamped()
         msg.header.frame_id = 'map'
         msg.pose = pose
         return msg
 
-    def publishGoalPose(self, goal_pose: Optional[Pose] = None):
+    def publishGoalPose(self, goal_pose: Optional[Pose] = None) -> None:
         self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
         self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose))
 
-    def runNavigateAction(self, goal_pose: Optional[Pose] = None):
+    def runNavigateAction(self, goal_pose: Optional[Pose] = None) -> bool:
         # Sends a `NavToPose` action request and waits for completion
         self.info_msg("Waiting for 'NavigateToPose' action server")
         while not self.action_client.wait_for_server(timeout_sec=1.0):
             self.info_msg("'NavigateToPose' action server not available, waiting...")
 
+        if os.getenv('GROOT_MONITORING') == 'True':
+            if not self.grootMonitoringGetStatus():
+                self.error_msg('Behavior Tree must not be running already!')
+                self.error_msg('Are you running multiple goals/bts..?')
+                return False
+
         self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
         goal_msg = NavigateToPose.Goal()
         goal_msg.pose = self.getStampedPoseMsg(self.goal_pose)
@@ -105,7 +109,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
         rclpy.spin_until_future_complete(self, send_goal_future)
         goal_handle = send_goal_future.result()
 
-        if not goal_handle.accepted:
+        if not goal_handle or not goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -113,12 +117,23 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
         get_result_future = goal_handle.get_result_async()
 
         future_return = True
+        if os.getenv('GROOT_MONITORING') == 'True':
+            try:
+                if not self.grootMonitoringReloadTree():
+                    self.error_msg('Failed GROOT_BT - Reload Tree from ZMQ Server')
+                    future_return = False
+                if not self.grootMonitoringSetBreakpoint():
+                    self.error_msg('Failed GROOT_BT - Set Breakpoint from ZMQ Publisher')
+                    future_return = False
+            except Exception as e:  # noqa: B902
+                self.error_msg(f'Failed GROOT_BT - ZMQ Tests: {e}')
+                future_return = False
 
         self.info_msg("Waiting for 'NavigateToPose' action to complete")
         rclpy.spin_until_future_complete(self, get_result_future)
-        status = get_result_future.result().status
+        status = get_result_future.result().status  # type: ignore[union-attr]
         if status != GoalStatus.STATUS_SUCCEEDED:
-            result = get_result_future.result().result
+            result = get_result_future.result().result  # type: ignore[union-attr]
             self.info_msg(f'Goal failed with status code: {status}'
                           f' error code:{result.error_code}'
                           f' error msg:{result.error_msg}')
@@ -130,12 +145,143 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
         self.info_msg('Goal succeeded!')
         return True
 
-    def poseCallback(self, msg):
+    def grootMonitoringReloadTree(self) -> bool:
+        # ZeroMQ Context
+        context = zmq.Context()
+
+        sock = context.socket(zmq.REQ)
+        port = 1667  # default server port for groot monitoring
+        # # Set a Timeout so we do not spin till infinity
+        sock.setsockopt(zmq.RCVTIMEO, 1000)
+        # sock.setsockopt(zmq.LINGER, 0)
+
+        sock.connect(f'tcp://localhost:{port}')
+        self.info_msg(f'ZMQ Server Port:{port}')
+
+        # this should fail
+        try:
+            sock.recv()
+            self.error_msg('ZMQ Reload Tree Test 1/3 - This should have failed!')
+            # Only works when ZMQ server receives a request first
+            sock.close()
+            return False
+        except zmq.error.ZMQError:
+            self.info_msg('ZMQ Reload Tree Test 1/3: Check')
+        try:
+            # request tree from server
+            request_header = struct.pack('!BBI', 2, ord('T'), 12345)
+            sock.send(request_header)
+            # receive tree from server as flat_buffer
+            sock.recv_multipart()
+            self.info_msg('ZMQ Reload Tree Test 2/3: Check')
+        except zmq.error.Again:
+            self.info_msg('ZMQ Reload Tree Test 2/3 - Failed to load tree')
+            sock.close()
+            return False
+
+        # this should fail
+        try:
+            sock.recv()
+            self.error_msg('ZMQ Reload Tree Test 3/3 - This should have failed!')
+            # Tree should only be loadable ONCE after ZMQ server received a request
+            sock.close()
+            return False
+        except zmq.error.ZMQError:
+            self.info_msg('ZMQ Reload Tree Test 3/3: Check')
+
+        return True
+
+    def grootMonitoringSetBreakpoint(self) -> bool:
+        # ZeroMQ Context
+        context = zmq.Context()
+        # Define the socket using the 'Context'
+        sock = context.socket(zmq.REQ)
+        # Set a Timeout so we do not spin till infinity
+        sock.setsockopt(zmq.RCVTIMEO, 2000)
+        # sock.setsockopt(zmq.LINGER, 0)
+
+        port = 1667  # default publishing port for groot monitoring
+        sock.connect(f'tcp://127.0.0.1:{port}')
+        self.info_msg(f'ZMQ Publisher Port:{port}')
+
+        # Create header for the request
+        request_header = struct.pack('!BBI', 2, ord('I'), 12345)  # HOOK_INSERT
+        # Create JSON for the hook
+        hook_data = {
+            'enabled': True,
+            'uid': 9,  # Node ID
+            'mode': 0,  # 0 = BREAKPOINT, 1 = REPLACE
+            'once': False,
+            'desired_status': 'SUCCESS',  # Desired status
+            'position': 0,  # 0 = PRE, 1 = POST
+        }
+        hook_json = json.dumps(hook_data)
+
+        # Send the request
+        try:
+            sock.send_multipart([request_header, hook_json.encode('utf-8')])
+            reply = sock.recv_multipart()
+            if len(reply[0]) < 2:
+                self.error_msg('ZMQ - Incomplete reply received')
+                sock.close()
+                return False
+        except Exception as e:
+            self.error_msg(f'ZMQ - Error during request: {e}')
+            sock.close()
+            return False
+        self.info_msg('ZMQ - HOOK_INSERT request sent')
+        return True
+
+    def grootMonitoringGetStatus(self) -> bool:
+        # ZeroMQ Context
+        context = zmq.Context()
+
+        sock = context.socket(zmq.REQ)
+        port = 1667  # default server port for groot monitoring
+        # # Set a Timeout so we do not spin till infinity
+        sock.setsockopt(zmq.RCVTIMEO, 1000)
+        # sock.setsockopt(zmq.LINGER, 0)
+
+        sock.connect(f'tcp://localhost:{port}')
+        self.info_msg(f'ZMQ Server Port:{port}')
+
+        for request in range(3):
+            try:
+                # request tree from server
+                request_header = struct.pack('!BBI', 2, ord('S'), 12345)
+                sock.send(request_header)
+                # receive tree from server as flat_buffer
+                reply = sock.recv_multipart()
+                if len(reply[0]) < 6:
+                    self.error_msg('ZMQ - Incomplete reply received')
+                    sock.close()
+                    return False
+                # Decoding payload
+                payload = reply[1]
+                node_states = []
+                offset = 0
+                while offset < len(payload):
+                    node_uid, node_status = struct.unpack_from('!HB', payload, offset)
+                    offset += 3  # 2 bytes for UID, 1 byte for status
+                    node_states.append((node_uid, node_status))
+                # Get the status of the first node
+                node_uid, node_status = node_states[0]
+                if node_status != 0:
+                    self.error_msg('ZMQ - BT Not running')
+                    return False
+            except zmq.error.Again:
+                self.error_msg('ZMQ - Did not receive any status')
+                sock.close()
+                return False
+        self.info_msg('ZMQ - Did receive status')
+        return True
+
+    def poseCallback(self, msg: PoseWithCovarianceStamped) -> None:
         self.info_msg('Received amcl_pose')
         self.current_pose = msg.pose.pose
         self.initial_pose_received = True
 
-    def reachesGoal(self, timeout, distance):
+    def reachesGoal(self, timeout: float, distance: float) -> bool:
         goalReached = False
         start_time = time.time()
 
@@ -150,14 +296,17 @@ def reachesGoal(self, timeout, distance):
                     self.error_msg('Robot timed out reaching its goal!')
                     return False
 
-    def distanceFromGoal(self):
+        self.info_msg('Robot reached its goal!')
+        return True
+
+    def distanceFromGoal(self) -> float:
         d_x = self.current_pose.position.x - self.goal_pose.position.x
         d_y = self.current_pose.position.y - self.goal_pose.position.y
         distance = math.sqrt(d_x * d_x + d_y * d_y)
         self.info_msg(f'Distance from goal is: {distance}')
         return distance
 
-    def wait_for_node_active(self, node_name: str):
+    def wait_for_node_active(self, node_name: str) -> None:
         # Waits for the node within the tester namespace to become active
         self.info_msg(f'Waiting for {node_name} to become active')
         node_service = f'{node_name}/get_state'
@@ -171,7 +320,7 @@ def wait_for_node_active(self, node_name: str):
             future = state_client.call_async(req)
             rclpy.spin_until_future_complete(self, future)
             if future.result() is not None:
-                state = future.result().current_state.label
+                state = future.result().current_state.label  # type: ignore[union-attr]
                 self.info_msg(f'Result of get_state: {state}')
             else:
                 self.error_msg(
@@ -179,7 +328,7 @@ def wait_for_node_active(self, node_name: str):
                 )
             time.sleep(5)
 
-    def shutdown(self):
+    def shutdown(self) -> None:
         self.info_msg('Shutting down')
         self.action_client.destroy()
 
@@ -214,13 +363,13 @@ def shutdown(self):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-    def wait_for_initial_pose(self):
+    def wait_for_initial_pose(self) -> bool:
         self.initial_pose_received = False
         # If the initial pose is not received within 100 seconds, return False
         # this is because when setting a wrong initial pose, amcl_pose is not received
         # and the test will hang indefinitely
         start_time = time.time()
-        duration = 0
+        duration = 0.0
         while not self.initial_pose_received:
             self.info_msg('Setting initial pose')
             self.setInitialPose()
@@ -233,14 +382,14 @@ def wait_for_initial_pose(self):
         return True
 
 
-def test_RobotMovesToGoal(robot_tester):
+def test_RobotMovesToGoal(robot_tester: NavTester) -> bool:
     robot_tester.info_msg('Setting goal pose')
     robot_tester.publishGoalPose()
     robot_tester.info_msg('Waiting 60 seconds for robot to reach goal')
     return robot_tester.reachesGoal(timeout=60, distance=0.5)
 
 
-def run_all_tests(robot_tester):
+def run_all_tests(robot_tester: NavTester) -> bool:
     # set transforms to use_sim_time
     result = True
     if result:
@@ -263,7 +412,7 @@ def run_all_tests(robot_tester):
     return result
 
 
-def fwd_pose(x=0.0, y=0.0, z=0.01):
+def fwd_pose(x: float = 0.0, y: float = 0.0, z: float = 0.01) -> Pose:
     initial_pose = Pose()
     initial_pose.position.x = x
     initial_pose.position.y = y
@@ -275,7 +424,7 @@ def fwd_pose(x=0.0, y=0.0, z=0.01):
     return initial_pose
 
 
-def get_testers(args):
+def get_testers(args: argparse.Namespace) -> list[NavTester]:
     testers = []
 
     if args.robot:
@@ -323,7 +472,7 @@ def get_testers(args):
     return testers
 
 
-def check_args(expect_failure: str):
+def check_args(expect_failure: str) -> Any:
     # Check if --expect_failure is True or False
     if expect_failure != 'True' and expect_failure != 'False':
         print(
@@ -334,7 +483,7 @@ def check_args(expect_failure: str):
         return eval(expect_failure)
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     # The robot(s) positions from the input arguments
     parser = argparse.ArgumentParser(description='System-level navigation tester node')
     parser.add_argument('-e', '--expect_failure')
@@ -353,7 +502,7 @@ def main(argv=sys.argv[1:]):
         action='append',
         nargs=5,
         metavar=('name', 'init_x', 'init_y', 'final_x', 'final_y'),
-        help="The robot's namespace and starting and final positions. "
+        help="The robot's namespace and starting and final positions."
         + 'Repeating the argument for multiple robots is supported.',
     )
 
diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py
index 612c050a043..e39a6993f77 100755
--- a/nav2_system_tests/src/system/test_multi_robot_launch.py
+++ b/nav2_system_tests/src/system/test_multi_robot_launch.py
@@ -18,31 +18,25 @@
 import sys
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription, LaunchService
-from launch.actions import (
-    ExecuteProcess,
-    GroupAction,
-    IncludeLaunchDescription,
-    SetEnvironmentVariable,
-)
+from launch.actions import (ExecuteProcess, GroupAction, IncludeLaunchDescription,
+                            SetEnvironmentVariable)
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import TextSubstitution
 from launch_ros.actions import Node, PushROSNamespace
-
 from launch_testing.legacy import LaunchTestService
 
 
-def generate_launch_description():
-    map_yaml_file = os.getenv('TEST_MAP')
-    world = os.getenv('TEST_WORLD')
-    urdf = os.getenv('TEST_URDF')
-    sdf = os.getenv('TEST_SDF')
+def generate_launch_description() -> LaunchDescription:
+    map_yaml_file = os.getenv('TEST_MAP', '')
+    world = os.getenv('TEST_WORLD', '')
+    urdf = os.getenv('TEST_URDF', '')
+    sdf = os.getenv('TEST_SDF', '')
 
     bt_xml_file = os.path.join(
         get_package_share_directory('nav2_bt_navigator'),
         'behavior_trees',
-        os.getenv('BT_NAVIGATOR_XML'),
+        os.getenv('BT_NAVIGATOR_XML', ''),
     )
 
     bringup_dir = get_package_share_directory('nav2_bringup')
@@ -163,13 +157,13 @@ def generate_launch_description():
     return ld
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
     # TODO(orduno) remove duplicated definition of robots on `generate_launch_description`
     test1_action = ExecuteProcess(
         cmd=[
-            os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')),
+            os.path.join(os.getenv('TEST_DIR', ''), os.getenv('TESTER', '')),
             '-rs',
             'robot1',
             '0.0',
diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py
index fb4d3625880..e93efc9a646 100755
--- a/nav2_system_tests/src/system/test_system_launch.py
+++ b/nav2_system_tests/src/system/test_system_launch.py
@@ -20,24 +20,17 @@
 import sys
 
 from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import (
-    AppendEnvironmentVariable,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    SetEnvironmentVariable,
-)
+from launch import LaunchDescription, LaunchService
+from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription,
+                            SetEnvironmentVariable)
 from launch.launch_context import LaunchContext
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_testing.legacy import LaunchTestService
-
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
 
@@ -53,7 +46,7 @@ def generate_launch_description():
     bt_navigator_xml = os.path.join(
         get_package_share_directory('nav2_bt_navigator'),
         'behavior_trees',
-        os.getenv('BT_NAVIGATOR_XML'),
+        os.getenv('BT_NAVIGATOR_XML', ''),
     )
 
     # Use local param file
@@ -68,11 +61,14 @@ def generate_launch_description():
     if os.getenv('ASTAR') == 'True':
         param_substitutions.update({'use_astar': 'True'})
 
+    if (os.getenv('GROOT_MONITORING') == 'True'):
+        param_substitutions.update({'enable_groot_monitoring': 'True'})
+
     param_substitutions.update(
-        {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')}
+        {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')}
     )
     param_substitutions.update(
-        {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')}
+        {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')}
     )
 
     configured_params = RewrittenYaml(
@@ -141,12 +137,12 @@ def generate_launch_description():
     )
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
     test1_action = ExecuteProcess(
         cmd=[
-            os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')),
+            os.path.join(os.getenv('TEST_DIR', ''), os.getenv('TESTER', '')),
             '-r',
             '-2.0',
             '-0.5',
diff --git a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py
index b31f19bdd61..8413da01458 100755
--- a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py
+++ b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py
@@ -20,24 +20,17 @@
 import sys
 
 from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import (
-    AppendEnvironmentVariable,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    SetEnvironmentVariable,
-)
+from launch import LaunchDescription, LaunchService
+from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription,
+                            SetEnvironmentVariable)
 from launch.launch_context import LaunchContext
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_testing.legacy import LaunchTestService
-
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     nav2_system_tests_dir = get_package_share_directory('nav2_system_tests')
@@ -54,7 +47,7 @@ def generate_launch_description():
     bt_navigator_xml = os.path.join(
         get_package_share_directory('nav2_bt_navigator'),
         'behavior_trees',
-        os.getenv('BT_NAVIGATOR_XML'),
+        os.getenv('BT_NAVIGATOR_XML', ''),
     )
 
     # Use local param file
@@ -70,10 +63,10 @@ def generate_launch_description():
         param_substitutions.update({'use_astar': 'True'})
 
     param_substitutions.update(
-        {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')}
+        {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')}
     )
     param_substitutions.update(
-        {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')}
+        {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')}
     )
 
     configured_params = RewrittenYaml(
@@ -154,12 +147,12 @@ def generate_launch_description():
     )
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
     test1_action = ExecuteProcess(
         cmd=[
-            os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')),
+            os.path.join(os.getenv('TEST_DIR', ''), os.getenv('TESTER', '')),
             '-r',
             '-2.0',
             '-0.5',
diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py
index 31e1b8af411..81ae8bd483c 100755
--- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py
+++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py
@@ -20,23 +20,17 @@
 import sys
 
 from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import (
-    AppendEnvironmentVariable,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    SetEnvironmentVariable,
-)
+from launch import LaunchDescription, LaunchService
+from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription,
+                            SetEnvironmentVariable)
 from launch.launch_context import LaunchContext
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_testing.legacy import LaunchTestService
-
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
 
@@ -52,7 +46,7 @@ def generate_launch_description():
     bt_navigator_xml = os.path.join(
         get_package_share_directory('nav2_bt_navigator'),
         'behavior_trees',
-        os.getenv('BT_NAVIGATOR_XML'),
+        os.getenv('BT_NAVIGATOR_XML', ''),
     )
 
     # Use local param file
@@ -67,11 +61,14 @@ def generate_launch_description():
     if os.getenv('ASTAR') == 'True':
         param_substitutions.update({'use_astar': 'True'})
 
+    if (os.getenv('GROOT_MONITORING') == 'True'):
+        param_substitutions.update({'enable_groot_monitoring': 'True'})
+
     param_substitutions.update(
-        {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')}
+        {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')}
     )
     param_substitutions.update(
-        {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')}
+        {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')}
     )
 
     configured_params = RewrittenYaml(
@@ -140,12 +137,12 @@ def generate_launch_description():
     )
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
     test1_action = ExecuteProcess(
         cmd=[
-            os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')),
+            os.path.join(os.getenv('TEST_DIR', ''), os.getenv('TESTER', '')),
             '-r',
             '-200000.0',
             '-200000.0',
diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py
index 0cbc76533e2..8b8e2542c6d 100755
--- a/nav2_system_tests/src/system_failure/test_system_failure_launch.py
+++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py
@@ -20,24 +20,17 @@
 import sys
 
 from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import (
-    AppendEnvironmentVariable,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    SetEnvironmentVariable,
-)
+from launch import LaunchDescription, LaunchService
+from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription,
+                            SetEnvironmentVariable)
 from launch.launch_context import LaunchContext
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_testing.legacy import LaunchTestService
-
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
 
@@ -53,7 +46,7 @@ def generate_launch_description():
     bt_navigator_xml = os.path.join(
         get_package_share_directory('nav2_bt_navigator'),
         'behavior_trees',
-        os.getenv('BT_NAVIGATOR_XML'),
+        os.getenv('BT_NAVIGATOR_XML', ''),
     )
 
     params_file = os.path.join(nav2_bringup_dir, 'params', 'nav2_params.yaml')
@@ -129,12 +122,12 @@ def generate_launch_description():
     )
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
     test1_action = ExecuteProcess(
         cmd=[
-            os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'),
+            os.path.join(os.getenv('TEST_DIR', ''), 'tester_node.py'),
             '-r',
             '-2.0',
             '-0.5',
diff --git a/nav2_system_tests/src/system_failure/tester_node.py b/nav2_system_tests/src/system_failure/tester_node.py
index 9cea2f6118d..615f24f27ec 100755
--- a/nav2_system_tests/src/system_failure/tester_node.py
+++ b/nav2_system_tests/src/system_failure/tester_node.py
@@ -21,19 +21,14 @@
 from typing import Optional
 
 from action_msgs.msg import GoalStatus
-from geometry_msgs.msg import Pose
-from geometry_msgs.msg import PoseStamped
-from geometry_msgs.msg import PoseWithCovarianceStamped
+from geometry_msgs.msg import Pose, PoseStamped, PoseWithCovarianceStamped
 from lifecycle_msgs.srv import GetState
 from nav2_msgs.action import NavigateToPose
 from nav2_msgs.srv import ManageLifecycleNodes
-
 import rclpy
-
-from rclpy.action import ActionClient
+from rclpy.action import ActionClient  # type: ignore[attr-defined]
 from rclpy.node import Node
-from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
-from rclpy.qos import QoSProfile
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
 
 
 class NavTester(Node):
@@ -60,16 +55,16 @@ def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''):
         self.goal_pose = goal_pose
         self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
 
-    def info_msg(self, msg: str):
+    def info_msg(self, msg: str) -> None:
         self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
 
-    def warn_msg(self, msg: str):
+    def warn_msg(self, msg: str) -> None:
         self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m')
 
-    def error_msg(self, msg: str):
+    def error_msg(self, msg: str) -> None:
         self.get_logger().error('\033[1;37;41m' + msg + '\033[0m')
 
-    def setInitialPose(self):
+    def setInitialPose(self) -> None:
         msg = PoseWithCovarianceStamped()
         msg.pose.pose = self.initial_pose
         msg.header.frame_id = 'map'
@@ -77,17 +72,17 @@ def setInitialPose(self):
         self.initial_pose_pub.publish(msg)
         self.currentPose = self.initial_pose
 
-    def getStampedPoseMsg(self, pose: Pose):
+    def getStampedPoseMsg(self, pose: Pose) -> PoseStamped:
         msg = PoseStamped()
         msg.header.frame_id = 'map'
         msg.pose = pose
         return msg
 
-    def publishGoalPose(self, goal_pose: Optional[Pose] = None):
+    def publishGoalPose(self, goal_pose: Optional[Pose] = None) -> None:
         self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
         self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose))
 
-    def runNavigateAction(self, goal_pose: Optional[Pose] = None):
+    def runNavigateAction(self, goal_pose: Optional[Pose] = None) -> bool:
         # Sends a `NavToPose` action request and waits for completion
         self.info_msg("Waiting for 'NavigateToPose' action server")
         while not self.action_client.wait_for_server(timeout_sec=1.0):
@@ -103,7 +98,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
         rclpy.spin_until_future_complete(self, send_goal_future)
         goal_handle = send_goal_future.result()
 
-        if not goal_handle.accepted:
+        if not goal_handle or not goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -112,7 +107,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
 
         self.info_msg("Waiting for 'NavigateToPose' action to complete")
         rclpy.spin_until_future_complete(self, get_result_future)
-        status = get_result_future.result().status
+        status = get_result_future.result().status  # type: ignore[union-attr]
         if status != GoalStatus.STATUS_ABORTED:
             self.info_msg(f'Goal failed with status code: {status}')
             return False
@@ -120,12 +115,12 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
         self.info_msg('Goal failed, as expected!')
         return True
 
-    def poseCallback(self, msg):
+    def poseCallback(self, msg: PoseWithCovarianceStamped) -> None:
         self.info_msg('Received amcl_pose')
         self.current_pose = msg.pose.pose
         self.initial_pose_received = True
 
-    def reachesGoal(self, timeout, distance):
+    def reachesGoal(self, timeout: float, distance: float) -> bool:
         goalReached = False
         start_time = time.time()
 
@@ -140,14 +135,17 @@ def reachesGoal(self, timeout, distance):
                     self.error_msg('Robot timed out reaching its goal!')
                     return False
 
-    def distanceFromGoal(self):
+        self.info_msg('Robot reached its goal!')
+        return True
+
+    def distanceFromGoal(self) -> float:
         d_x = self.current_pose.position.x - self.goal_pose.position.x
         d_y = self.current_pose.position.y - self.goal_pose.position.y
         distance = math.sqrt(d_x * d_x + d_y * d_y)
         self.info_msg(f'Distance from goal is: {distance}')
         return distance
 
-    def wait_for_node_active(self, node_name: str):
+    def wait_for_node_active(self, node_name: str) -> None:
         # Waits for the node within the tester namespace to become active
         self.info_msg(f'Waiting for {node_name} to become active')
         node_service = f'{node_name}/get_state'
@@ -161,7 +159,7 @@ def wait_for_node_active(self, node_name: str):
             future = state_client.call_async(req)
             rclpy.spin_until_future_complete(self, future)
             if future.result() is not None:
-                state = future.result().current_state.label
+                state = future.result().current_state.label  # type: ignore[union-attr]
                 self.info_msg(f'Result of get_state: {state}')
             else:
                 self.error_msg(
@@ -169,7 +167,7 @@ def wait_for_node_active(self, node_name: str):
                 )
             time.sleep(5)
 
-    def shutdown(self):
+    def shutdown(self) -> None:
         self.info_msg('Shutting down')
         self.action_client.destroy()
 
@@ -204,7 +202,7 @@ def shutdown(self):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-    def wait_for_initial_pose(self):
+    def wait_for_initial_pose(self) -> None:
         self.initial_pose_received = False
         while not self.initial_pose_received:
             self.info_msg('Setting initial pose')
@@ -213,7 +211,7 @@ def wait_for_initial_pose(self):
             rclpy.spin_once(self, timeout_sec=1)
 
 
-def run_all_tests(robot_tester):
+def run_all_tests(robot_tester: NavTester) -> bool:
     # set transforms to use_sim_time
     result = True
     if result:
@@ -232,7 +230,7 @@ def run_all_tests(robot_tester):
     return result
 
 
-def fwd_pose(x=0.0, y=0.0, z=0.01):
+def fwd_pose(x: float = 0.0, y: float = 0.0, z: float = 0.01) -> Pose:
     initial_pose = Pose()
     initial_pose.position.x = x
     initial_pose.position.y = y
@@ -244,7 +242,7 @@ def fwd_pose(x=0.0, y=0.0, z=0.01):
     return initial_pose
 
 
-def get_testers(args):
+def get_testers(args: argparse.Namespace) -> list[NavTester]:
     testers = []
 
     if args.robot:
@@ -271,7 +269,7 @@ def get_testers(args):
     return testers
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     # The robot(s) positions from the input arguments
     parser = argparse.ArgumentParser(description='System-level navigation tester node')
     group = parser.add_mutually_exclusive_group(required=True)
diff --git a/nav2_system_tests/src/updown/README.md b/nav2_system_tests/src/updown/README.md
index 43564543e1e..24a3b35bb17 100644
--- a/nav2_system_tests/src/updown/README.md
+++ b/nav2_system_tests/src/updown/README.md
@@ -1,6 +1,6 @@
 # Nav2 Updown Test
 
-This is a 'top level' system test which tests the lifecycle bringup and shutdown of the system. 
+This is a 'top level' system test which tests the lifecycle bringup and shutdown of the system.
 
 ## To run the test
 ```
diff --git a/nav2_system_tests/src/updown/test_updown_launch.py b/nav2_system_tests/src/updown/test_updown_launch.py
index 81e0cc1fda0..e9312e3ae8b 100755
--- a/nav2_system_tests/src/updown/test_updown_launch.py
+++ b/nav2_system_tests/src/updown/test_updown_launch.py
@@ -14,15 +14,13 @@
 
 import os
 
-from ament_index_python.packages import get_package_prefix
-from ament_index_python.packages import get_package_share_directory
-
+from ament_index_python.packages import get_package_prefix, get_package_share_directory
 import launch.actions
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> launch.LaunchDescription:
 
     # Configuration parameters for the launch
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
diff --git a/nav2_system_tests/src/updown/test_updown_reliability b/nav2_system_tests/src/updown/test_updown_reliability
index 18388443d89..72bd182bc31 100755
--- a/nav2_system_tests/src/updown/test_updown_reliability
+++ b/nav2_system_tests/src/updown/test_updown_reliability
@@ -3,7 +3,7 @@ for i in `seq 1 1000`;
 do
     echo "======= START OF RUN: $i ========="
 
-    # Start with a new ros2 daemon 
+    # Start with a new ros2 daemon
     ros2 daemon stop
 
     # There shouldn't be any nodes in the list
diff --git a/nav2_system_tests/src/updown/updownresults.py b/nav2_system_tests/src/updown/updownresults.py
index de38c51c8bf..1106267179f 100755
--- a/nav2_system_tests/src/updown/updownresults.py
+++ b/nav2_system_tests/src/updown/updownresults.py
@@ -28,7 +28,7 @@
 import sys
 
 
-def main():
+def main() -> None:
     log = sys.stdin
     test_count = 0
     fail_count = 0
diff --git a/nav2_system_tests/src/waypoint_follower/test_case_launch.py b/nav2_system_tests/src/waypoint_follower/test_case_launch.py
index ea7f581eab3..0c92849d806 100755
--- a/nav2_system_tests/src/waypoint_follower/test_case_launch.py
+++ b/nav2_system_tests/src/waypoint_follower/test_case_launch.py
@@ -18,24 +18,17 @@
 import sys
 
 from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch import LaunchService
-from launch.actions import (
-    AppendEnvironmentVariable,
-    ExecuteProcess,
-    IncludeLaunchDescription,
-    SetEnvironmentVariable,
-)
+from launch import LaunchDescription, LaunchService
+from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription,
+                            SetEnvironmentVariable)
 from launch.launch_context import LaunchContext
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_testing.legacy import LaunchTestService
-
 from nav2_common.launch import RewrittenYaml
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
 
@@ -51,7 +44,7 @@ def generate_launch_description():
     bt_navigator_xml = os.path.join(
         get_package_share_directory('nav2_bt_navigator'),
         'behavior_trees',
-        os.getenv('BT_NAVIGATOR_XML'),
+        os.getenv('BT_NAVIGATOR_XML', '')
     )
 
     params_file = os.path.join(nav2_bringup_dir, 'params/nav2_params.yaml')
@@ -119,11 +112,11 @@ def generate_launch_description():
     ])
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     ld = generate_launch_description()
 
     test1_action = ExecuteProcess(
-        cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester.py')],
+        cmd=[os.path.join(os.getenv('TEST_DIR', ''), 'tester.py')],
         name='tester_node',
         output='screen')
 
diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py
index 85dba0be401..c70198c5e8f 100755
--- a/nav2_system_tests/src/waypoint_follower/tester.py
+++ b/nav2_system_tests/src/waypoint_follower/tester.py
@@ -15,33 +15,35 @@
 
 import sys
 import time
+from typing import Optional
 
 from action_msgs.msg import GoalStatus
 from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
 from nav2_msgs.action import ComputePathToPose, FollowWaypoints
 from nav2_msgs.srv import ManageLifecycleNodes
 from rcl_interfaces.srv import SetParameters
-
 import rclpy
-from rclpy.action import ActionClient
+from rclpy.action import ActionClient  # type: ignore[attr-defined]
+from rclpy.action.client import ClientGoalHandle
 from rclpy.node import Node
 from rclpy.parameter import Parameter
-from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
-from rclpy.qos import QoSProfile
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
 
 
 class WaypointFollowerTest(Node):
 
-    def __init__(self):
+    def __init__(self) -> None:
         super().__init__(node_name='nav2_waypoint_tester', namespace='')
-        self.waypoints = None
+        self.waypoints: list[float] = []
         self.action_client = ActionClient(self, FollowWaypoints, 'follow_waypoints')
         self.initial_pose_pub = self.create_publisher(
             PoseWithCovarianceStamped, 'initialpose', 10
         )
         self.initial_pose_received = False
-        self.goal_handle = None
-        self.action_result = None
+        self.goal_handle: Optional[ClientGoalHandle[
+                FollowWaypoints.Goal, FollowWaypoints.Result,
+                FollowWaypoints.Feedback]] = None
+        self.action_result = FollowWaypoints.Result()
 
         pose_qos = QoSProfile(
             durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
@@ -57,7 +59,7 @@ def __init__(self):
             SetParameters, '/waypoint_follower/set_parameters'
         )
 
-    def setInitialPose(self, pose):
+    def setInitialPose(self, pose: list[float]) -> None:
         self.init_pose = PoseWithCovarianceStamped()
         self.init_pose.pose.pose.position.x = pose[0]
         self.init_pose.pose.pose.position.y = pose[1]
@@ -65,11 +67,11 @@ def setInitialPose(self, pose):
         self.publishInitialPose()
         time.sleep(5)
 
-    def poseCallback(self, msg):
+    def poseCallback(self, msg: PoseWithCovarianceStamped) -> None:
         self.info_msg('Received amcl_pose')
         self.initial_pose_received = True
 
-    def setWaypoints(self, waypoints):
+    def setWaypoints(self, waypoints: list[list[float]]) -> None:
         self.waypoints = []
         for wp in waypoints:
             msg = PoseStamped()
@@ -79,7 +81,7 @@ def setWaypoints(self, waypoints):
             msg.pose.orientation.w = 1.0
             self.waypoints.append(msg)
 
-    def run(self, block, cancel):
+    def run(self, block: bool, cancel: bool) -> bool:
         # if not self.waypoints:
         #     rclpy.error_msg('Did not set valid waypoints before running test!')
         #     return False
@@ -98,7 +100,7 @@ def run(self, block, cancel):
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
 
-        if not self.goal_handle.accepted:
+        if not self.goal_handle or not self.goal_handle.accepted:
             self.error_msg('Goal rejected')
             return False
 
@@ -114,8 +116,8 @@ def run(self, block, cancel):
         self.info_msg("Waiting for 'follow_waypoints' action to complete")
         try:
             rclpy.spin_until_future_complete(self, get_result_future)
-            status = get_result_future.result().status
-            result = get_result_future.result().result
+            status = get_result_future.result().status  # type: ignore[union-attr]
+            result = get_result_future.result().result  # type: ignore[union-attr]
             self.action_result = result
         except Exception as e:  # noqa: B902
             self.error_msg(f'Service call failed {e!r}')
@@ -126,17 +128,17 @@ def run(self, block, cancel):
         if len(self.action_result.missed_waypoints) > 0:
             self.info_msg(
                 'Goal failed to process all waypoints,'
-                ' missed {0} wps.'.format(len(self.action_result.missed_waypoints))
+                f' missed {len(self.action_result.missed_waypoints)} wps.'
             )
             return False
 
         self.info_msg('Goal succeeded!')
         return True
 
-    def publishInitialPose(self):
+    def publishInitialPose(self) -> None:
         self.initial_pose_pub.publish(self.init_pose)
 
-    def setStopFailureParam(self, value):
+    def setStopFailureParam(self, value: bool) -> None:
         req = SetParameters.Request()
         req.parameters = [
             Parameter('stop_on_failure', Parameter.Type.BOOL, value).to_parameter_msg()
@@ -144,7 +146,7 @@ def setStopFailureParam(self, value):
         future = self.param_cli.call_async(req)
         rclpy.spin_until_future_complete(self, future)
 
-    def shutdown(self):
+    def shutdown(self) -> None:
         self.info_msg('Shutting down')
 
         self.action_client.destroy()
@@ -182,21 +184,21 @@ def shutdown(self):
 
         self.info_msg(f'{transition_service} finished')
 
-    def cancel_goal(self):
-        cancel_future = self.goal_handle.cancel_goal_async()
+    def cancel_goal(self) -> None:
+        cancel_future = self.goal_handle.cancel_goal_async()  # type: ignore[union-attr]
         rclpy.spin_until_future_complete(self, cancel_future)
 
-    def info_msg(self, msg: str):
+    def info_msg(self, msg: str) -> None:
         self.get_logger().info(msg)
 
-    def warn_msg(self, msg: str):
+    def warn_msg(self, msg: str) -> None:
         self.get_logger().warn(msg)
 
-    def error_msg(self, msg: str):
+    def error_msg(self, msg: str) -> None:
         self.get_logger().error(msg)
 
 
-def main(argv=sys.argv[1:]):
+def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
     rclpy.init()
 
     # wait a few seconds to make sure entire stacks are up
diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md
index 0ac1fa9b7aa..a88045d992f 100644
--- a/nav2_theta_star_planner/README.md
+++ b/nav2_theta_star_planner/README.md
@@ -3,7 +3,7 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav
 
 See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-thetastar.html) for additional parameter descriptions.
 
-## Features 
+## Features
 - The planner uses A\* search along with line of sight (LOS) checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\*
 - As it also considers the costmap traversal cost during execution it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases)
 - Uses the costs from the costmap to penalise high cost regions
@@ -51,7 +51,7 @@ w1*euc_cost(par, neigh) + w2*(costmap(par,neigh)/LETHAL_COST)^2`
 ## Parameters
 The parameters of the planner are :
 - ` .how_many_corners ` : to choose between 4-connected and 8-connected graph expansions, the accepted values are 4 and 8
-- ` .w_euc_cost ` : weight applied on the length of the path. 
+- ` .w_euc_cost ` : weight applied on the length of the path.
 - ` .w_traversal_cost ` : it tunes how harshly the nodes of high cost are penalised. From the above g(neigh) equation you can see that the cost-aware component of the cost function forms a parabolic curve, thus this parameter would, on increasing its value, make that curve steeper allowing for a greater differentiation (as the delta of costs would increase, when the graph becomes steep) among the nodes of different costs.
 Below are the default values of the parameters :
 ```
diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp
index d0b64898c1a..3cf10bbc05c 100644
--- a/nav2_theta_star_planner/test/test_theta_star.cpp
+++ b/nav2_theta_star_planner/test/test_theta_star.cpp
@@ -230,7 +230,7 @@ TEST(ThetaStarPlanner, test_theta_star_reconfigure)
     results);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_util/README.md b/nav2_util/README.md
index adf71e61c2e..1c25071190e 100644
--- a/nav2_util/README.md
+++ b/nav2_util/README.md
@@ -13,9 +13,9 @@ The long-term aim is for these utilities to find more permanent homes in other p
 
 ## Twist Publisher and Twist Subscriber for commanded velocities
 
-### Background 
+### Background
 
-The Twist Publisher and Twist Subscriber are utility classes to assist NAV2 transition from 
+The Twist Publisher and Twist Subscriber are utility classes to assist NAV2 transition from
 [Twist](https://github.com/ros2/common_interfaces/blob/humble/geometry_msgs/msg/Twist.msg) to [TwistStamped](https://github.com/ros2/common_interfaces/blob/humble/geometry_msgs/msg/TwistStamped.msg).
 
 Details on the migration are found in [#1594](https://github.com/ros-planning/navigation2/issues/1594).
@@ -28,4 +28,4 @@ The utility has the following effect:
 
 Every node in `nav2` that subscribes or publishes velocity commands with `Twist` now supports this optional behavior.
 The behavior up through ROS 2 Iron is preserved - using `Twist`. In a future ROS 2 version, when enough of the
-ROS ecosystem has moved to `TwistStamped`, the default may change. 
+ROS ecosystem has moved to `TwistStamped`, the default may change.
diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp
index 67cc87243d6..4b371c49c91 100644
--- a/nav2_util/include/nav2_util/geometry_utils.hpp
+++ b/nav2_util/include/nav2_util/geometry_utils.hpp
@@ -16,6 +16,7 @@
 #define NAV2_UTIL__GEOMETRY_UTILS_HPP_
 
 #include <cmath>
+#include <vector>
 
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
@@ -24,6 +25,7 @@
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "nav_msgs/msg/path.hpp"
+#include "nav2_msgs/msg/waypoint_status.hpp"
 
 namespace nav2_util
 {
@@ -179,6 +181,31 @@ inline double calculate_path_length(const nav_msgs::msg::Path & path, size_t sta
   return path_length;
 }
 
+/**
+ * @brief Find the index of the first goal in `PENDING` status matching the
+ * given target pose.
+ * @param waypoint_statuses List of waypoint statuses to search through.
+ * @param goal Target pose to match against waypoint goals.
+ * @return Index of the first matching goal in PENDING status, -1 if not found.
+ */
+inline int find_next_matching_goal_in_waypoint_statuses(
+  const std::vector<nav2_msgs::msg::WaypointStatus> & waypoint_statuses,
+  const geometry_msgs::msg::PoseStamped & goal)
+{
+  auto itr = std::find_if(
+    waypoint_statuses.begin(), waypoint_statuses.end(),
+    [&goal](const nav2_msgs::msg::WaypointStatus & status) {
+      return status.waypoint_pose == goal &&
+      status.waypoint_status == nav2_msgs::msg::WaypointStatus::PENDING;
+    });
+
+  if (itr == waypoint_statuses.end()) {
+    return -1;
+  }
+
+  return itr - waypoint_statuses.begin();
+}
+
 }  // namespace geometry_utils
 }  // namespace nav2_util
 
diff --git a/nav2_util/include/nav2_util/lifecycle_service_client.hpp b/nav2_util/include/nav2_util/lifecycle_service_client.hpp
index cceb5393684..46d322f7709 100644
--- a/nav2_util/include/nav2_util/lifecycle_service_client.hpp
+++ b/nav2_util/include/nav2_util/lifecycle_service_client.hpp
@@ -31,7 +31,8 @@ namespace nav2_util
 class LifecycleServiceClient
 {
 public:
-  explicit LifecycleServiceClient(const std::string & lifecycle_node_name);
+  explicit LifecycleServiceClient(
+    const std::string & lifecycle_node_name);
   LifecycleServiceClient(
     const std::string & lifecycle_node_name,
     rclcpp::Node::SharedPtr parent_node);
diff --git a/nav2_util/include/nav2_util/odometry_utils.hpp b/nav2_util/include/nav2_util/odometry_utils.hpp
index 8f184b1ae36..6017040cb4a 100644
--- a/nav2_util/include/nav2_util/odometry_utils.hpp
+++ b/nav2_util/include/nav2_util/odometry_utils.hpp
@@ -72,8 +72,9 @@ class OdomSmoother
   {
     std::lock_guard<std::mutex> lock(odom_mutex_);
     if (!received_odom_) {
-      RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"),
-          "OdomSmoother has not received any data yet, returning empty Twist");
+      RCLCPP_ERROR(
+        rclcpp::get_logger("OdomSmoother"),
+        "OdomSmoother has not received any data yet, returning empty Twist");
       geometry_msgs::msg::Twist twist;
       return twist;
     }
@@ -88,8 +89,9 @@ class OdomSmoother
   {
     std::lock_guard<std::mutex> lock(odom_mutex_);
     if (!received_odom_) {
-      RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"),
-          "OdomSmoother has not received any data yet, returning empty Twist");
+      RCLCPP_ERROR(
+        rclcpp::get_logger("OdomSmoother"),
+        "OdomSmoother has not received any data yet, returning empty Twist");
       geometry_msgs::msg::TwistStamped twist_stamped;
       return twist_stamped;
     }
@@ -104,25 +106,27 @@ class OdomSmoother
   {
     std::lock_guard<std::mutex> lock(odom_mutex_);
     if (!received_odom_) {
-      RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"),
-          "OdomSmoother has not received any data yet, returning empty Twist");
+      RCLCPP_ERROR(
+        rclcpp::get_logger("OdomSmoother"),
+        "OdomSmoother has not received any data yet, returning empty Twist");
       geometry_msgs::msg::Twist twist;
       return twist;
     }
     return odom_history_.back().twist.twist;
   }
 
-    /**
-   * @brief Get raw twist stamped msg from smoother (without smoothing)
-   * @return twist TwistStamped msg
-   */
+  /**
+ * @brief Get raw twist stamped msg from smoother (without smoothing)
+ * @return twist TwistStamped msg
+ */
   inline geometry_msgs::msg::TwistStamped getRawTwistStamped()
   {
     std::lock_guard<std::mutex> lock(odom_mutex_);
     geometry_msgs::msg::TwistStamped twist_stamped;
     if (!received_odom_) {
-      RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"),
-          "OdomSmoother has not received any data yet, returning empty Twist");
+      RCLCPP_ERROR(
+        rclcpp::get_logger("OdomSmoother"),
+        "OdomSmoother has not received any data yet, returning empty Twist");
       return twist_stamped;
     }
     twist_stamped.header = odom_history_.back().header;
diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp
index e76a2cce708..cffbe80a26b 100644
--- a/nav2_util/include/nav2_util/robot_utils.hpp
+++ b/nav2_util/include/nav2_util/robot_utils.hpp
@@ -146,7 +146,6 @@ bool getTransform(
  * @return True if valid, false if contains unactionable values
  */
 [[nodiscard]] bool validateTwist(const geometry_msgs::msg::Twist & msg);
-[[nodiscard]] bool validateTwist(const geometry_msgs::msg::TwistStamped & msg);
 
 }  // end namespace nav2_util
 
diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp
index 6f25965d1e3..6c27d4457f3 100644
--- a/nav2_util/include/nav2_util/service_client.hpp
+++ b/nav2_util/include/nav2_util/service_client.hpp
@@ -16,7 +16,8 @@
 #define NAV2_UTIL__SERVICE_CLIENT_HPP_
 
 #include <string>
-
+#include <memory>
+#include <chrono>
 #include "rclcpp/rclcpp.hpp"
 
 namespace nav2_util
@@ -24,7 +25,7 @@ namespace nav2_util
 
 /**
  * @class nav2_util::ServiceClient
- * @brief A simple wrapper on ROS2 services for invoke() and block-style calling
+ * @brief A simple wrapper on ROS2 services client
  */
 template<class ServiceT, typename NodeT = rclcpp::Node::SharedPtr>
 class ServiceClient
@@ -34,24 +35,46 @@ class ServiceClient
   * @brief A constructor
   * @param service_name name of the service to call
   * @param provided_node Node to create the service client off of
+  * @param use_internal_executor Whether to create an internal executor or not
   */
   explicit ServiceClient(
     const std::string & service_name,
-    const NodeT & provided_node)
-  : service_name_(service_name), node_(provided_node)
+    const NodeT & provided_node, bool use_internal_executor = false)
+  : service_name_(service_name), node_(provided_node), use_internal_executor_(use_internal_executor)
   {
-    callback_group_ = node_->create_callback_group(
-      rclcpp::CallbackGroupType::MutuallyExclusive,
-      false);
-    callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+    if (use_internal_executor) {
+      callback_group_ = node_->create_callback_group(
+        rclcpp::CallbackGroupType::MutuallyExclusive,
+        false);
+      callback_group_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+      callback_group_executor_->add_callback_group(
+        callback_group_,
+        node_->get_node_base_interface());
+    }
+    // When a nullptr is passed, the client will use the default callback group
     client_ = node_->template create_client<ServiceT>(
       service_name,
       rclcpp::SystemDefaultsQoS(),
       callback_group_);
+    rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF;
+    if (!node_->has_parameter("service_introspection_mode")) {
+      node_->declare_parameter("service_introspection_mode", "disabled");
+    }
+    std::string service_introspection_mode =
+      node_->get_parameter("service_introspection_mode").as_string();
+    if (service_introspection_mode == "metadata") {
+      introspection_state = RCL_SERVICE_INTROSPECTION_METADATA;
+    } else if (service_introspection_mode == "contents") {
+      introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS;
+    }
+
+    this->client_->configure_introspection(
+      node_->get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state);
   }
 
   using RequestType = typename ServiceT::Request;
   using ResponseType = typename ServiceT::Response;
+  using SharedPtr = std::shared_ptr<ServiceClient<ServiceT, NodeT>>;
 
   /**
   * @brief Invoke the service and block until completed or timed out
@@ -77,10 +100,7 @@ class ServiceClient
       node_->get_logger(), "%s service client: send async request",
       service_name_.c_str());
     auto future_result = client_->async_send_request(request);
-
-    if (callback_group_executor_.spin_until_future_complete(future_result, timeout) !=
-      rclcpp::FutureReturnCode::SUCCESS)
-    {
+    if (spin_until_complete(future_result, timeout) != rclcpp::FutureReturnCode::SUCCESS) {
       // Pending request must be manually cleaned up if execution is interrupted or timed out
       client_->remove_pending_request(future_result);
       throw std::runtime_error(service_name_ + " service client: async_send_request failed");
@@ -113,10 +133,7 @@ class ServiceClient
       node_->get_logger(), "%s service client: send async request",
       service_name_.c_str());
     auto future_result = client_->async_send_request(request);
-
-    if (callback_group_executor_.spin_until_future_complete(future_result) !=
-      rclcpp::FutureReturnCode::SUCCESS)
-    {
+    if (spin_until_complete(future_result) != rclcpp::FutureReturnCode::SUCCESS) {
       // Pending request must be manually cleaned up if execution is interrupted or timed out
       client_->remove_pending_request(future_result);
       return false;
@@ -126,6 +143,30 @@ class ServiceClient
     return response.get();
   }
 
+  /**
+  * @brief Asynchronously call the service
+  * @param request The request object to call the service using
+  * @return std::shared_future<typename ResponseType::SharedPtr> The shared future of the service response
+  */
+  std::shared_future<typename ResponseType::SharedPtr> async_call(
+    typename RequestType::SharedPtr & request)
+  {
+    auto future_result = client_->async_send_request(request);
+    return future_result.share();
+  }
+
+
+  /**
+  * @brief Asynchronously call the service with a callback
+  * @param request The request object to call the service using
+  * @param callback The callback to call when the service response is received
+  */
+  template<typename CallbackT>
+  void async_call(typename RequestType::SharedPtr request, CallbackT && callback)
+  {
+    client_->async_send_request(request, callback);
+  }
+
   /**
   * @brief Block until a service is available or timeout
   * @param timeout Maximum timeout to wait for, default infinite
@@ -136,6 +177,25 @@ class ServiceClient
     return client_->wait_for_service(timeout);
   }
 
+  /**
+   * @brief Spins the executor until the provided future is complete or the timeout is reached.
+   *
+   * @param future The shared future to wait for completion.
+   * @param timeout The maximum time to wait for the future to complete. Default is -1 (no timeout).
+   * @return rclcpp::FutureReturnCode indicating the result of the spin operation.
+   */
+  template<typename FutureT>
+  rclcpp::FutureReturnCode spin_until_complete(
+    const FutureT & future,
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1))
+  {
+    if (use_internal_executor_) {
+      return callback_group_executor_->spin_until_future_complete(future, timeout);
+    } else {
+      return rclcpp::spin_until_future_complete(node_, future, timeout);
+    }
+  }
+
   /**
   * @brief Gets the service name
   * @return string Service name
@@ -148,9 +208,10 @@ class ServiceClient
 protected:
   std::string service_name_;
   NodeT node_;
-  rclcpp::CallbackGroup::SharedPtr callback_group_;
-  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr};
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr callback_group_executor_;
   typename rclcpp::Client<ServiceT>::SharedPtr client_;
+  bool use_internal_executor_;
 };
 
 }  // namespace nav2_util
diff --git a/nav2_util/include/nav2_util/service_server.hpp b/nav2_util/include/nav2_util/service_server.hpp
new file mode 100644
index 00000000000..4402154ae56
--- /dev/null
+++ b/nav2_util/include/nav2_util/service_server.hpp
@@ -0,0 +1,82 @@
+// Copyright (c) 2025 Maurice Alexander Purnawan
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_UTIL__SERVICE_SERVER_HPP_
+#define NAV2_UTIL__SERVICE_SERVER_HPP_
+
+#include <string>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+namespace nav2_util
+{
+
+/**
+ * @class nav2_util::ServiceServer
+ * @brief A simple wrapper on ROS2 services server
+ */
+template<class ServiceT, typename NodeT = rclcpp::Node::SharedPtr>
+class ServiceServer
+{
+public:
+  using RequestType = typename ServiceT::Request;
+  using ResponseType = typename ServiceT::Response;
+  using CallbackType = std::function<void (const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<RequestType>, std::shared_ptr<ResponseType>)>;
+  using SharedPtr = std::shared_ptr<ServiceServer<ServiceT, NodeT>>;
+
+  explicit ServiceServer(
+    const std::string & service_name,
+    const NodeT & node,
+    CallbackType callback,
+    const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
+    rclcpp::CallbackGroup::SharedPtr callback_group = nullptr)
+  : service_name_(service_name), callback_(callback)
+  {
+    server_ = node->template create_service<ServiceT>(
+      service_name,
+      [this](const std::shared_ptr<rmw_request_id_t> request_header,
+      const std::shared_ptr<RequestType> request, std::shared_ptr<ResponseType> response) {
+        this->callback_(request_header, request, response);
+      },
+      qos,
+      callback_group);
+
+    rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF;
+    if (!node->has_parameter("service_introspection_mode")) {
+      node->declare_parameter("service_introspection_mode", "disabled");
+    }
+    std::string service_introspection_mode =
+      node->get_parameter("service_introspection_mode").as_string();
+    if (service_introspection_mode == "metadata") {
+      introspection_state = RCL_SERVICE_INTROSPECTION_METADATA;
+    } else if (service_introspection_mode == "contents") {
+      introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS;
+    }
+
+    this->server_->configure_introspection(
+      node->get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state);
+  }
+
+protected:
+  std::string service_name_;
+  CallbackType callback_;
+  typename rclcpp::Service<ServiceT>::SharedPtr server_;
+};
+
+}  // namespace nav2_util
+
+
+#endif  // NAV2_UTIL__SERVICE_SERVER_HPP_
diff --git a/nav2_util/src/lifecycle_service_client.cpp b/nav2_util/src/lifecycle_service_client.cpp
index 70656d9f297..9674e76e090 100644
--- a/nav2_util/src/lifecycle_service_client.cpp
+++ b/nav2_util/src/lifecycle_service_client.cpp
@@ -30,10 +30,13 @@ using namespace std::chrono_literals;
 namespace nav2_util
 {
 
-LifecycleServiceClient::LifecycleServiceClient(const string & lifecycle_node_name)
+LifecycleServiceClient::LifecycleServiceClient(
+  const string & lifecycle_node_name)
 : node_(generate_internal_node(lifecycle_node_name + "_lifecycle_client")),
-  change_state_(lifecycle_node_name + "/change_state", node_),
-  get_state_(lifecycle_node_name + "/get_state", node_)
+  change_state_(lifecycle_node_name + "/change_state", node_,
+    true /*creates and spins an internal executor*/),
+  get_state_(lifecycle_node_name + "/get_state", node_,
+    true /*creates and spins an internal executor*/)
 {
   // Block until server is up
   rclcpp::Rate r(20);
@@ -48,8 +51,10 @@ LifecycleServiceClient::LifecycleServiceClient(
   const string & lifecycle_node_name,
   rclcpp::Node::SharedPtr parent_node)
 : node_(parent_node),
-  change_state_(lifecycle_node_name + "/change_state", node_),
-  get_state_(lifecycle_node_name + "/get_state", node_)
+  change_state_(lifecycle_node_name + "/change_state", node_,
+    true /*creates and spins an internal executor*/),
+  get_state_(lifecycle_node_name + "/get_state", node_,
+    true /*creates and spins an internal executor*/)
 {
   // Block until server is up
   rclcpp::Rate r(20);
diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp
index 736709ce641..e9e26a9fdb5 100644
--- a/nav2_util/src/robot_utils.cpp
+++ b/nav2_util/src/robot_utils.cpp
@@ -202,9 +202,4 @@ bool validateTwist(const geometry_msgs::msg::Twist & msg)
   return true;
 }
 
-bool validateTwist(const geometry_msgs::msg::TwistStamped & msg)
-{
-  return validateTwist(msg.twist);
-}
-
 }  // end namespace nav2_util
diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt
index 14d774c2986..9ba40dfe512 100644
--- a/nav2_util/test/CMakeLists.txt
+++ b/nav2_util/test/CMakeLists.txt
@@ -10,6 +10,9 @@ find_package(test_msgs REQUIRED)
 ament_add_gtest(test_service_client test_service_client.cpp)
 target_link_libraries(test_service_client ${library_name} ${std_srvs_TARGETS})
 
+ament_add_gtest(test_service_server test_service_server.cpp)
+target_link_libraries(test_service_server ${library_name} ${std_srvs_TARGETS})
+
 ament_add_gtest(test_string_utils test_string_utils.cpp)
 target_link_libraries(test_string_utils ${library_name})
 
diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp
index 0ae2ffe0c0e..1482fbe0fa1 100644
--- a/nav2_util/test/test_actions.cpp
+++ b/nav2_util/test/test_actions.cpp
@@ -114,7 +114,7 @@ class FibonacciServerNode : public rclcpp::Node
         return;
       }
 
-      // Check if we've gotten an new goal, pre-empting the current one
+      // Check if we've gotten an new goal, preempting the current one
       if (do_premptions_ && action_server_->is_preempt_requested()) {
         action_server_->accept_pending_goal();
         goto preempted;
diff --git a/nav2_util/test/test_base_footprint_publisher.cpp b/nav2_util/test/test_base_footprint_publisher.cpp
index a3cdda664f0..3dfc65726c9 100644
--- a/nav2_util/test/test_base_footprint_publisher.cpp
+++ b/nav2_util/test/test_base_footprint_publisher.cpp
@@ -63,7 +63,7 @@ TEST(TestBaseFootprintPublisher, TestBaseFootprintPublisher)
   EXPECT_EQ(t.transform.translation.z, 0.0);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_util/test/test_geometry_utils.cpp b/nav2_util/test/test_geometry_utils.cpp
index 38f27ad3044..fdfa828dfad 100644
--- a/nav2_util/test/test_geometry_utils.cpp
+++ b/nav2_util/test/test_geometry_utils.cpp
@@ -21,6 +21,7 @@
 
 using nav2_util::geometry_utils::euclidean_distance;
 using nav2_util::geometry_utils::calculate_path_length;
+using nav2_util::geometry_utils::find_next_matching_goal_in_waypoint_statuses;
 
 TEST(GeometryUtils, euclidean_distance_point_3d)
 {
@@ -128,3 +129,34 @@ TEST(GeometryUtils, calculate_path_length)
     calculate_path_length(circle_path),
     2 * pi * polar_distance, 1e-1);
 }
+
+TEST(GeometryUtils, find_next_matching_goal_in_waypoint_statuses)
+{
+  size_t nb_waypoints = 10;
+  float distance_between_waypoints = 2.0;
+
+  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses(nb_waypoints);
+  for (size_t i = 0; i < nb_waypoints; ++i) {
+    waypoint_statuses[i].waypoint_index = i;
+    waypoint_statuses[i].waypoint_pose.pose.position.x = distance_between_waypoints * i;
+  }
+
+  // match success
+  size_t matching_index = nb_waypoints / 2;
+  ASSERT_EQ(
+    find_next_matching_goal_in_waypoint_statuses(
+      waypoint_statuses,
+      waypoint_statuses[matching_index].waypoint_pose), matching_index);
+
+  // match failed due to mismatch of pose
+  geometry_msgs::msg::PoseStamped fake_pose;
+  fake_pose.pose.position.x = -1.0;
+  ASSERT_EQ(find_next_matching_goal_in_waypoint_statuses(waypoint_statuses, fake_pose), -1);
+
+  // match failed due to waypoint_status is not PENDING
+  waypoint_statuses[matching_index].waypoint_status = nav2_msgs::msg::WaypointStatus::COMPLETED;
+  ASSERT_EQ(
+    find_next_matching_goal_in_waypoint_statuses(
+      waypoint_statuses,
+      waypoint_statuses[matching_index].waypoint_pose), -1);
+}
diff --git a/nav2_util/test/test_lifecycle_cli_node.cpp b/nav2_util/test/test_lifecycle_cli_node.cpp
index 0efc9f69697..0fd9529c66d 100644
--- a/nav2_util/test/test_lifecycle_cli_node.cpp
+++ b/nav2_util/test/test_lifecycle_cli_node.cpp
@@ -93,7 +93,7 @@ TEST(LifecycleCLI, succeeds_node_name)
   SUCCEED();
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_util/test/test_lifecycle_utils.cpp b/nav2_util/test/test_lifecycle_utils.cpp
index f6a5076082b..89a7e1155ec 100644
--- a/nav2_util/test/test_lifecycle_utils.cpp
+++ b/nav2_util/test/test_lifecycle_utils.cpp
@@ -51,7 +51,7 @@ TEST(Lifecycle, interface)
   SUCCEED();
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_util/test/test_node_utils.cpp b/nav2_util/test/test_node_utils.cpp
index fa23b7867e9..d7ffb3cf5d5 100644
--- a/nav2_util/test/test_node_utils.cpp
+++ b/nav2_util/test/test_node_utils.cpp
@@ -87,7 +87,7 @@ TEST(GetPluginTypeParam, GetPluginTypeParam)
   EXPECT_THROW(get_plugin_type_param(node, "Waldo"), std::runtime_error);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_util/test/test_odometry_utils.cpp b/nav2_util/test/test_odometry_utils.cpp
index e799ebf223d..c279f8a6daa 100644
--- a/nav2_util/test/test_odometry_utils.cpp
+++ b/nav2_util/test/test_odometry_utils.cpp
@@ -151,7 +151,7 @@ TEST(OdometryUtils, test_smoothed_velocity)
   EXPECT_EQ(twist_raw_msg.angular.z, 5.0);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_util/test/test_service_client.cpp b/nav2_util/test/test_service_client.cpp
index 521d3f30568..7ffc48a2689 100644
--- a/nav2_util/test/test_service_client.cpp
+++ b/nav2_util/test/test_service_client.cpp
@@ -36,8 +36,10 @@ class TestServiceClient : public ServiceClient<std_srvs::srv::Empty>
 public:
   TestServiceClient(
     const std::string & name,
-    const rclcpp::Node::SharedPtr & provided_node = rclcpp::Node::SharedPtr())
-  : ServiceClient(name, provided_node) {}
+    const rclcpp::Node::SharedPtr & provided_node = rclcpp::Node::SharedPtr(),
+    bool use_internal_executor = false)
+  : ServiceClient(name, provided_node, use_internal_executor)
+  {}
 
   string name() {return node_->get_name();}
   const rclcpp::Node::SharedPtr & getNode() {return node_;}
@@ -45,10 +47,16 @@ class TestServiceClient : public ServiceClient<std_srvs::srv::Empty>
 
 TEST(ServiceClient, can_ServiceClient_use_passed_in_node)
 {
-  auto node = rclcpp::Node::make_shared("test_node");
-  TestServiceClient t("bar", node);
-  ASSERT_EQ(t.getNode(), node);
-  ASSERT_EQ(t.name(), "test_node");
+  std::vector<std::string> introspection_modes = {
+    "disabled", "metadata", "contents"
+  };
+  for (const auto & mode : introspection_modes) {
+    auto node = rclcpp::Node::make_shared("test_node" + mode);
+    node->declare_parameter("service_introspection_mode", mode);
+    TestServiceClient t("bar", node, true);
+    ASSERT_EQ(t.getNode(), node);
+    ASSERT_EQ(t.name(), "test_node" + mode);
+  }
 }
 
 TEST(ServiceClient, can_ServiceClient_invoke_in_callback)
@@ -69,7 +77,7 @@ TEST(ServiceClient, can_ServiceClient_invoke_in_callback)
   auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node);});
 
   auto sub_node = rclcpp::Node::make_shared("sub_node");
-  ServiceClient<std_srvs::srv::Empty> client("empty_srv", sub_node);
+  ServiceClient<std_srvs::srv::Empty> client("empty_srv", sub_node, true);
   auto sub = sub_node->create_subscription<std_msgs::msg::Empty>(
     "empty_topic",
     rclcpp::QoS(1),
@@ -91,9 +99,41 @@ TEST(ServiceClient, can_ServiceClient_timeout)
 {
   rclcpp::init(0, nullptr);
   auto node = rclcpp::Node::make_shared("test_node");
-  TestServiceClient t("bar", node);
+  TestServiceClient t("bar", node, true);
   rclcpp::spin_some(node);
   bool ready = t.wait_for_service(std::chrono::milliseconds(10));
   rclcpp::shutdown();
   ASSERT_EQ(ready, false);
 }
+
+TEST(ServiceClient, can_ServiceClient_async_call) {
+  rclcpp::init(0, nullptr);
+
+  int a = 0;
+  bool callback_called = false;
+  // Define service server
+  auto service_node = rclcpp::Node::make_shared("service_node");
+  auto service = service_node->create_service<std_srvs::srv::Empty>(
+    "empty_srv",
+    [&a](std_srvs::srv::Empty::Request::SharedPtr, std_srvs::srv::Empty::Response::SharedPtr) {
+      a = 1;
+    });
+  auto srv_thread = std::thread([&]() {rclcpp::spin(service_node);});
+  // Define service client
+  auto node = rclcpp::Node::make_shared("test_node");
+  ServiceClient<std_srvs::srv::Empty> client("empty_srv", node, false);
+  auto req = std::make_shared<std_srvs::srv::Empty::Request>();
+  auto callback =
+    [&callback_called](rclcpp::Client<std_srvs::srv::Empty>::SharedFuture /*future*/) {
+      callback_called = true;
+    };
+  // Test async_call
+  client.async_call(req, callback);
+  std::this_thread::sleep_for(std::chrono::seconds(1));
+  rclcpp::spin_some(node);
+
+  rclcpp::shutdown();
+  srv_thread.join();
+  ASSERT_EQ(a, 1);
+  ASSERT_TRUE(callback_called);
+}
diff --git a/nav2_util/test/test_service_server.cpp b/nav2_util/test/test_service_server.cpp
new file mode 100644
index 00000000000..2808ef79a24
--- /dev/null
+++ b/nav2_util/test/test_service_server.cpp
@@ -0,0 +1,82 @@
+// Copyright (c) 2025 Maurice Alexander Purnawan
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <string>
+#include "nav2_util/service_server.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "std_srvs/srv/empty.hpp"
+#include "std_msgs/msg/empty.hpp"
+#include "gtest/gtest.h"
+
+using nav2_util::ServiceServer;
+using std::string;
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class TestServiceServer : public ServiceServer<std_srvs::srv::Empty>
+{
+public:
+  TestServiceServer(
+    const std::string & name,
+    const rclcpp::Node::SharedPtr & provided_node,
+    CallbackType callback)
+  : ServiceServer(name, provided_node, callback)
+  {}
+};
+
+TEST(ServiceServer, can_handle_all_introspection_modes)
+{
+  std::vector<std::string> introspection_modes = {
+    "disabled", "metadata", "contents"
+  };
+
+  for (const auto & mode : introspection_modes) {
+    int a = 0;
+    auto node = rclcpp::Node::make_shared("test_node_" + mode);
+
+    node->declare_parameter("service_introspection_mode", mode);
+
+    auto callback = [&a](const std::shared_ptr<rmw_request_id_t>,
+        const std::shared_ptr<std_srvs::srv::Empty::Request>,
+        std::shared_ptr<std_srvs::srv::Empty::Response>) {
+        a = 1;
+      };
+
+    TestServiceServer server("empty_srv_" + mode, node, callback);
+
+    auto client_node = rclcpp::Node::make_shared("client_node_" + mode);
+    auto client = client_node->create_client<std_srvs::srv::Empty>("empty_srv_" + mode);
+
+    ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1)));
+
+    auto req = std::make_shared<std_srvs::srv::Empty::Request>();
+    auto result = client->async_send_request(req);
+    rclcpp::spin_some(node);
+    rclcpp::spin_some(client_node);
+    ASSERT_EQ(a, 1);
+  }
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/nav2_velocity_smoother/README.md b/nav2_velocity_smoother/README.md
index 579b3a6737a..cd5c0548653 100644
--- a/nav2_velocity_smoother/README.md
+++ b/nav2_velocity_smoother/README.md
@@ -3,7 +3,7 @@
 The ``nav2_velocity_smoother`` is a package containing a lifecycle-component node for smoothing velocities sent by Nav2 to robot controllers.
 The aim of this package is to implement velocity, acceleration, and deadband smoothing from Nav2 to reduce wear-and-tear on robot motors and hardware controllers by smoothing out the accelerations/jerky movements that might be present with some local trajectory planners' control efforts.
 
-It supports differential drive and omnidirectional robot platforms primarily, but is applicable to ackermann as well with some interpretations of ``Twist``. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). 
+It supports differential drive and omnidirectional robot platforms primarily, but is applicable to ackermann as well with some interpretations of ``Twist``. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/).
 
 See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-velocity-smoother.html) for additional parameter descriptions.
 
@@ -87,4 +87,4 @@ The minimum and maximum velocities for rotation (e.g. ``Vw``) represent left and
 
 Deadband velocities are minimum thresholds, below which we set its value to `0`. This can be useful when your robot's breaking torque from stand still is non-trivial so sending very small values will pull high amounts of current.
 
-The `VelocitySmoother` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).
\ No newline at end of file
+The `VelocitySmoother` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).
diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp
index 00144bb5769..de1f6d53771 100644
--- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp
+++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp
@@ -708,7 +708,7 @@ TEST(VelocitySmootherTest, testDynamicParameter)
   smoother.reset();
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_voxel_grid/README.md b/nav2_voxel_grid/README.md
index a10d519e2f0..9aec987ce79 100644
--- a/nav2_voxel_grid/README.md
+++ b/nav2_voxel_grid/README.md
@@ -1,9 +1,9 @@
 # Nav2 Voxel Grid
 
-The `nav2_voxel_grid` package contains the VoxelGrid used by the `Voxel Layer` inside of `nav2_costmap_2d`. The voxel grid itself is simply a 2D char pointer array of the map size with bit locations corresponding to voxel values (free, unknown, occupied , etc). 
+The `nav2_voxel_grid` package contains the VoxelGrid used by the `Voxel Layer` inside of `nav2_costmap_2d`. The voxel grid itself is simply a 2D char pointer array of the map size with bit locations corresponding to voxel values (free, unknown, occupied , etc).
 
-It is branched out as a separate package for use in other applications where a dense voxel grid representation may be useful. It also contains implementations of 3D raycasting. 
+It is branched out as a separate package for use in other applications where a dense voxel grid representation may be useful. It also contains implementations of 3D raycasting.
 
 ## ROS1 Comparison
 
-This package is a direct port to ROS2 for use in the voxel layer. 
+This package is a direct port to ROS2 for use in the voxel layer.
diff --git a/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp b/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp
index cce36e7fc48..6219ff41c09 100644
--- a/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp
+++ b/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp
@@ -69,7 +69,7 @@ TEST(voxel_grid, bresenham3DBoundariesCheck)
   nav2_voxel_grid::VoxelGrid vg(sz_x, sz_y, sz_z);
   TestVoxel tv(vg.getData(), sz_x, sz_y);
 
-  // Initial point - some assymetrically standing point in order to cover most corner cases
+  // Initial point - some asymmetrically standing point in order to cover most corner cases
   const double x0 = 2.2;
   const double y0 = 3.8;
   const double z0 = 0.4;
diff --git a/nav2_waypoint_follower/README.md b/nav2_waypoint_follower/README.md
index 822314fbf39..c906eba2ff9 100644
--- a/nav2_waypoint_follower/README.md
+++ b/nav2_waypoint_follower/README.md
@@ -33,10 +33,10 @@ Neither is better than the other, it highly depends on the tasks your robot(s) a
 `nav2_waypoint_follower` provides an action server named `FollowGPSWaypoints` which accepts GPS waypoint following requests by using tools provided by `robot_localization` and `nav2_waypoint_follower` itself.
 
 `robot_localization`'s `navsat_transform_node` provides a service `fromLL`, which is used to convert pure GPS coordinates(longitude, latitude, alitude)
-to cartesian coordinates in map frame(x,y), then the existent action named `FollowWaypoints` from `nav2_waypoint_follower` is used to get robot go through each converted waypoints. 
+to cartesian coordinates in map frame(x,y), then the existent action named `FollowWaypoints` from `nav2_waypoint_follower` is used to get robot go through each converted waypoints.
 The action msg definition for GPS waypoint following can be found [here](../nav2_msgs/action/FollowGPSWaypoints.action).
 
-In a common use case, an client node can read a set of GPS waypoints from a YAML file an create a client to action server named as `FollowGPSWaypoints`.  
+In a common use case, an client node can read a set of GPS waypoints from a YAML file an create a client to action server named as `FollowGPSWaypoints`.
 For instance,
 
 ```cpp
@@ -45,4 +45,4 @@ rclcpp_action::Client<ClientT>::SharedPtr gps_waypoint_follower_action_client_;
 gps_waypoint_follower_action_client_ = rclcpp_action::create_client<ClientT>(this, "follow_gps_waypoints");
 ```
 
-All other functionalities provided by `nav2_waypoint_follower` such as WaypointTaskExecutors are usable and can be configured in WaypointTaskExecutor.
\ No newline at end of file
+All other functionalities provided by `nav2_waypoint_follower` such as WaypointTaskExecutors are usable and can be configured in WaypointTaskExecutor.
diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp
index 2a8537007e2..8be79fcb8e0 100644
--- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp
+++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp
@@ -28,7 +28,7 @@ namespace nav2_waypoint_follower
 {
 
 /**
- * @brief Simple plugin based on WaypointTaskExecutor, lets robot to wait for a
+ * @brief Simple plugin based on WaypointTaskExecutor, let's robot to wait for a
  *        user input at waypoint arrival.
  */
 class InputAtWaypoint : public nav2_core::WaypointTaskExecutor
diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp
index 69498aa267a..c6e6ab09fd0 100644
--- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp
+++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp
@@ -26,7 +26,7 @@ namespace nav2_waypoint_follower
 {
 
 /**
- * @brief Simple plugin based on WaypointTaskExecutor, lets robot to sleep for a
+ * @brief Simple plugin based on WaypointTaskExecutor, let's robot to sleep for a
  *        specified amount of time at waypoint arrival. You can reference this class to define
  *        your own task and rewrite the body for it.
  *
diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp
index 578b33b1339..24e0948c8a0 100644
--- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp
+++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp
@@ -27,7 +27,7 @@
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_msgs/action/navigate_to_pose.hpp"
 #include "nav2_msgs/action/follow_waypoints.hpp"
-#include "nav2_msgs/msg/missed_waypoint.hpp"
+#include "nav2_msgs/msg/waypoint_status.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_util/simple_action_server.hpp"
 #include "nav2_util/node_utils.hpp"
diff --git a/nav2_waypoint_follower/plugins.xml b/nav2_waypoint_follower/plugins.xml
index ab8c01f050f..fe9f497c175 100644
--- a/nav2_waypoint_follower/plugins.xml
+++ b/nav2_waypoint_follower/plugins.xml
@@ -1,18 +1,18 @@
 <class_libraries>
   <library path="wait_at_waypoint">
     <class type="nav2_waypoint_follower::WaitAtWaypoint" base_class_type="nav2_core::WaypointTaskExecutor">
-      <description>Lets robot sleep for a specified amount of time at waypoint arrival</description>
+      <description>Let's robot sleep for a specified amount of time at waypoint arrival</description>
     </class>
   </library>
   <library path="photo_at_waypoint">
     <class type="nav2_waypoint_follower::PhotoAtWaypoint" base_class_type="nav2_core::WaypointTaskExecutor">
-      <description>Run-time plugin that takes photos at waypoint arrivals when using waypoint follower node. 
+      <description>Run-time plugin that takes photos at waypoint arrivals when using waypoint follower node.
         Saves the taken photos to specified directory.</description>
     </class>
   </library>
   <library path="input_at_waypoint">
     <class type="nav2_waypoint_follower::InputAtWaypoint" base_class_type="nav2_core::WaypointTaskExecutor">
-      <description>Lets robot wait for input at waypoint arrival</description>
+      <description>Let's robot wait for input at waypoint arrival</description>
     </class>
   </library>
 </class_libraries>
diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp
index 55db83df847..dcf025aa3ce 100644
--- a/nav2_waypoint_follower/src/waypoint_follower.cpp
+++ b/nav2_waypoint_follower/src/waypoint_follower.cpp
@@ -96,7 +96,8 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & state)
     nav2_util::ServiceClient<robot_localization::srv::FromLL,
     std::shared_ptr<nav2_util::LifecycleNode>>>(
     "/fromLL",
-    node);
+    node,
+    true /*creates and spins an internal executor*/);
 
   gps_action_server_ = std::make_unique<ActionServerGPS>(
     get_node_base_interface(),
@@ -305,9 +306,10 @@ void WaypointFollower::followWaypointsHandler(
       current_goal_status_.status == ActionStatus::FAILED ||
       current_goal_status_.status == ActionStatus::UNKNOWN)
     {
-      nav2_msgs::msg::MissedWaypoint missedWaypoint;
-      missedWaypoint.index = goal_index;
-      missedWaypoint.goal = poses[goal_index];
+      nav2_msgs::msg::WaypointStatus missedWaypoint;
+      missedWaypoint.waypoint_status = nav2_msgs::msg::WaypointStatus::FAILED;
+      missedWaypoint.waypoint_index = goal_index;
+      missedWaypoint.waypoint_pose = poses[goal_index];
       missedWaypoint.error_code = current_goal_status_.error_code;
       missedWaypoint.error_msg = current_goal_status_.error_msg;
       result->missed_waypoints.push_back(missedWaypoint);
@@ -340,9 +342,10 @@ void WaypointFollower::followWaypointsHandler(
         is_task_executed ? "succeeded" : "failed!");
 
       if (!is_task_executed) {
-        nav2_msgs::msg::MissedWaypoint missedWaypoint;
-        missedWaypoint.index = goal_index;
-        missedWaypoint.goal = poses[goal_index];
+        nav2_msgs::msg::WaypointStatus missedWaypoint;
+        missedWaypoint.waypoint_status = nav2_msgs::msg::WaypointStatus::FAILED;
+        missedWaypoint.waypoint_index = goal_index;
+        missedWaypoint.waypoint_pose = poses[goal_index];
         missedWaypoint.error_code =
           nav2_msgs::action::FollowWaypoints::Result::TASK_EXECUTOR_FAILED;
         missedWaypoint.error_msg = "Task execution failed";
diff --git a/nav2_waypoint_follower/test/test_dynamic_parameters.cpp b/nav2_waypoint_follower/test/test_dynamic_parameters.cpp
index 046de92fd66..7f4ab0690f0 100644
--- a/nav2_waypoint_follower/test/test_dynamic_parameters.cpp
+++ b/nav2_waypoint_follower/test/test_dynamic_parameters.cpp
@@ -82,7 +82,7 @@ TEST(WPTest, test_dynamic_parameters)
   follower.reset();
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/nav2_waypoint_follower/test/test_task_executors.cpp b/nav2_waypoint_follower/test/test_task_executors.cpp
index 9b58d043120..a1946f79b12 100644
--- a/nav2_waypoint_follower/test/test_task_executors.cpp
+++ b/nav2_waypoint_follower/test/test_task_executors.cpp
@@ -154,7 +154,7 @@ TEST(WaypointFollowerTest, PhotoAtWaypoint)
   EXPECT_TRUE(paw->processAtWaypoint(pose, 0));
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 
diff --git a/tools/.codespell_ignore_words b/tools/.codespell_ignore_words
new file mode 100644
index 00000000000..41230c8f09e
--- /dev/null
+++ b/tools/.codespell_ignore_words
@@ -0,0 +1,15 @@
+master
+uint
+jupyter
+blacklist
+whitelist
+stdio
+deque
+thirdparty
+dur
+segway
+ue
+hist
+nav2
+ot
+ws
diff --git a/tools/bt2img.py b/tools/bt2img.py
index ab8307b2138..f0404d958a2 100755
--- a/tools/bt2img.py
+++ b/tools/bt2img.py
@@ -17,7 +17,7 @@
 # for instructions
 
 import argparse
-import xml.etree.ElementTree
+import xml.etree.ElementTree as ET
 
 import graphviz  # pip3 install graphviz
 
@@ -75,16 +75,13 @@
     'SubTree',
 ]
 
-global xml_tree
 
-
-def main():
-    global xml_tree
+def main() -> None:
     args = parse_command_line()
-    xml_tree = xml.etree.ElementTree.parse(args.behavior_tree)
+    xml_tree = ET.parse(args.behavior_tree)
     root_tree_name = find_root_tree_name(xml_tree)
     behavior_tree = find_behavior_tree(xml_tree, root_tree_name)
-    dot = convert2dot(behavior_tree)
+    dot = convert2dot(behavior_tree, xml_tree)
     if args.legend:
         legend = make_legend()
         legend.format = 'png'
@@ -96,7 +93,7 @@ def main():
     dot.render(args.image_out, view=args.display)
 
 
-def parse_command_line():
+def parse_command_line() -> argparse.Namespace:
     parser = argparse.ArgumentParser(
         description='Convert a behavior tree XML file to an image'
     )
@@ -124,11 +121,15 @@ def parse_command_line():
     return parser.parse_args()
 
 
-def find_root_tree_name(xml_tree):
-    return xml_tree.getroot().get('main_tree_to_execute')
+def find_root_tree_name(xml_tree: ET.ElementTree) -> str:
+    root = xml_tree.getroot()
+    main_tree = root.get('main_tree_to_execute')
+    if main_tree is None:
+        raise RuntimeError('No main_tree_to_execute attribute found in XML root')
+    return main_tree
 
 
-def find_behavior_tree(xml_tree, tree_name):
+def find_behavior_tree(xml_tree: ET.ElementTree, tree_name: str) -> ET.Element:
     trees = xml_tree.findall('BehaviorTree')
     if len(trees) == 0:
         raise RuntimeError('No behavior trees were found in the XML file')
@@ -141,33 +142,50 @@ def find_behavior_tree(xml_tree, tree_name):
 
 
 # Generate a dot description of the root of the behavior tree.
-def convert2dot(behavior_tree):
+def convert2dot(behavior_tree: ET.Element, xml_tree: ET.ElementTree) -> graphviz.Digraph:
     dot = graphviz.Digraph()
     root = behavior_tree
     parent_dot_name = str(hash(root))
     dot.node(parent_dot_name, root.get('ID'), shape='box')
-    convert_subtree(dot, root, parent_dot_name)
+    convert_subtree(dot, root, parent_dot_name, xml_tree)
     return dot
 
 
 # Recursive function. We add the children to the dot file, and then recursively
 # call this function on the children. Nodes are given an ID that is the hash
 # of the node to ensure each is unique.
-def convert_subtree(dot, parent_node, parent_dot_name):
+def convert_subtree(
+    dot: graphviz.Digraph,
+    parent_node: ET.Element,
+    parent_dot_name: str,
+    xml_tree: ET.ElementTree,
+) -> None:
     if parent_node.tag == 'SubTree':
-        add_sub_tree(dot, parent_dot_name, parent_node)
+        add_sub_tree(dot, parent_dot_name, parent_node, xml_tree)
     else:
-        add_nodes(dot, parent_dot_name, parent_node)
+        add_nodes(dot, parent_dot_name, parent_node, xml_tree)
 
 
-def add_sub_tree(dot, parent_dot_name, parent_node):
+def add_sub_tree(
+    dot: graphviz.Digraph,
+    parent_dot_name: str,
+    parent_node: ET.Element,
+    xml_tree: ET.ElementTree,
+) -> None:
     root_tree_name = parent_node.get('ID')
+    if root_tree_name is None:
+        raise RuntimeError('SubTree node has no ID attribute')
     dot.node(parent_dot_name, root_tree_name, shape='box')
     behavior_tree = find_behavior_tree(xml_tree, root_tree_name)
-    convert_subtree(dot, behavior_tree, parent_dot_name)
+    convert_subtree(dot, behavior_tree, parent_dot_name, xml_tree)
 
 
-def add_nodes(dot, parent_dot_name, parent_node):
+def add_nodes(
+    dot: graphviz.Digraph,
+    parent_dot_name: str,
+    parent_node: ET.Element,
+    xml_tree: ET.ElementTree,
+) -> None:
     for node in list(parent_node):
         label = make_label(node)
         dot.node(
@@ -179,12 +197,12 @@ def add_nodes(dot, parent_dot_name, parent_node):
         )
         dot_name = str(hash(node))
         dot.edge(parent_dot_name, dot_name)
-        convert_subtree(dot, node, dot_name)
+        convert_subtree(dot, node, dot_name, xml_tree)
 
 
 # The node label contains the:
 # type, the name if provided, and the parameters.
-def make_label(node):
+def make_label(node: ET.Element) -> str:
     label = "< <table border='0' cellspacing='0' cellpadding='0'>"
     label += f"<tr><td align='text'><i>{node.tag}</i></td></tr>"
     name = node.get('name')
@@ -197,7 +215,7 @@ def make_label(node):
     return label
 
 
-def node_color(node_type):
+def node_color(node_type: str) -> str:
     if node_type in control_nodes:
         return 'chartreuse4'
     if node_type in action_nodes:
@@ -213,7 +231,7 @@ def node_color(node_type):
 
 
 # creates a legend which can be provided with the other images.
-def make_legend():
+def make_legend() -> graphviz.Digraph:
     legend = graphviz.Digraph(graph_attr={'rankdir': 'LR'})
     legend.attr(label='Legend')
     legend.node('Unknown', shape='box', style='filled', color='grey')
diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py
index 0ae0f883efe..86cb4379b5f 100644
--- a/tools/planner_benchmarking/metrics.py
+++ b/tools/planner_benchmarking/metrics.py
@@ -23,11 +23,15 @@
 from geometry_msgs.msg import PoseStamped
 from nav2_simple_commander.robot_navigator import BasicNavigator
 import numpy as np
+from numpy.typing import NDArray
 import rclpy
+from rclpy.time import Time
 from transforms3d.euler import euler2quat
 
 
-def getPlannerResults(navigator, initial_pose, goal_pose, planners):
+def getPlannerResults(
+        navigator: BasicNavigator, initial_pose: PoseStamped,
+        goal_pose: PoseStamped, planners: list[str]) -> list[PoseStamped]:
     results = []
     for planner in planners:
         path = navigator._getPathImpl(initial_pose, goal_pose, planner, use_start=True)
@@ -39,7 +43,9 @@ def getPlannerResults(navigator, initial_pose, goal_pose, planners):
     return results
 
 
-def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res):
+def getRandomStart(
+        costmap: NDArray[np.float32], max_cost: int,
+        side_buffer: int, time_stamp: Time, res: float) -> PoseStamped:
     start = PoseStamped()
     start.header.frame_id = 'map'
     start.header.stamp = time_stamp
@@ -61,7 +67,9 @@ def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res):
     return start
 
 
-def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res):
+def getRandomGoal(
+        costmap: NDArray[np.float32], start: PoseStamped,
+        max_cost: int, side_buffer: int, time_stamp: Time, res: float) -> PoseStamped:
     goal = PoseStamped()
     goal.header.frame_id = 'map'
     goal.header.stamp = time_stamp
@@ -91,7 +99,7 @@ def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res):
     return goal
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()
@@ -110,7 +118,7 @@ def main():
     max_cost = 210
     side_buffer = 100
     time_stamp = navigator.get_clock().now().to_msg()
-    results = []
+    results: list[PoseStamped] = []
     seed(33)
 
     random_pairs = 100
diff --git a/tools/planner_benchmarking/planning_benchmark_bringup.py b/tools/planner_benchmarking/planning_benchmark_bringup.py
index 05c61a6284d..666c0997696 100644
--- a/tools/planner_benchmarking/planning_benchmark_bringup.py
+++ b/tools/planner_benchmarking/planning_benchmark_bringup.py
@@ -21,7 +21,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     config = os.path.join(
         get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml'
diff --git a/tools/planner_benchmarking/process_data.py b/tools/planner_benchmarking/process_data.py
index 299997774db..a53c771d142 100644
--- a/tools/planner_benchmarking/process_data.py
+++ b/tools/planner_benchmarking/process_data.py
@@ -18,12 +18,15 @@
 import pickle
 
 import matplotlib.pylab as plt
+from nav2_msgs.action import ComputePathToPose
+from nav2_msgs.msg import Costmap
+from nav_msgs.msg import Path
 import numpy as np
 import seaborn as sns
 from tabulate import tabulate
 
 
-def getPaths(results):
+def getPaths(results: list[ComputePathToPose.Result]) -> list[Path]:
     paths = []
     for result in results:
         for path in result:
@@ -31,7 +34,7 @@ def getPaths(results):
     return paths
 
 
-def getTimes(results):
+def getTimes(results: list[ComputePathToPose.Result]) -> list[float]:
     times = []
     for result in results:
         for time in result:
@@ -39,7 +42,7 @@ def getTimes(results):
     return times
 
 
-def getMapCoordsFromPaths(paths, resolution):
+def getMapCoordsFromPaths(paths: list[Path], resolution: float) -> list[list[float]]:
     coords = []
     for path in paths:
         x = []
@@ -52,8 +55,8 @@ def getMapCoordsFromPaths(paths, resolution):
     return coords
 
 
-def getPathLength(path):
-    path_length = 0
+def getPathLength(path: Path) -> float:
+    path_length = 0.0
     x_prev = path.poses[0].pose.position.x
     y_prev = path.poses[0].pose.position.y
     for i in range(1, len(path.poses)):
@@ -67,7 +70,7 @@ def getPathLength(path):
     return path_length
 
 
-def plotResults(costmap, paths):
+def plotResults(costmap: Costmap, paths: list[Path]) -> None:
     coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution)
     data = np.asarray(costmap.data)
     data.resize(costmap.metadata.size_y, costmap.metadata.size_x)
@@ -82,12 +85,14 @@ def plotResults(costmap, paths):
     plt.show()
 
 
-def averagePathCost(paths, costmap, num_of_planners):
+def averagePathCost(
+        paths: list[Path], costmap: Costmap,
+        num_of_planners: int) -> list[list[float]]:
     coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution)
     data = np.asarray(costmap.data)
     data.resize(costmap.metadata.size_y, costmap.metadata.size_x)
 
-    average_path_costs = []
+    average_path_costs: list[list[float]] = []
     for i in range(num_of_planners):
         average_path_costs.append([])
 
@@ -102,12 +107,12 @@ def averagePathCost(paths, costmap, num_of_planners):
     return average_path_costs
 
 
-def maxPathCost(paths, costmap, num_of_planners):
+def maxPathCost(paths: list[Path], costmap: Costmap, num_of_planners: int) -> list[list[float]]:
     coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution)
     data = np.asarray(costmap.data)
     data.resize(costmap.metadata.size_y, costmap.metadata.size_x)
 
-    max_path_costs = []
+    max_path_costs: list[list[float]] = []
     for i in range(num_of_planners):
         max_path_costs.append([])
 
@@ -124,7 +129,7 @@ def maxPathCost(paths, costmap, num_of_planners):
     return max_path_costs
 
 
-def main():
+def main() -> None:
 
     print('Read data')
     with open(os.getcwd() + '/results.pickle', 'rb') as f:
@@ -137,18 +142,17 @@ def main():
         costmap = pickle.load(f)
 
     paths = getPaths(results)
-    path_lengths = []
+    path_lengths_list = []
 
     for path in paths:
-        path_lengths.append(getPathLength(path))
-    path_lengths = np.asarray(path_lengths)
+        path_lengths_list.append(getPathLength(path))
+    path_lengths = np.asarray(path_lengths_list)
     total_paths = len(paths)
 
     path_lengths.resize((int(total_paths / len(planners)), len(planners)))
     path_lengths = path_lengths.transpose()
 
-    times = getTimes(results)
-    times = np.asarray(times)
+    times = np.asarray(getTimes(results))
     times.resize((int(total_paths / len(planners)), len(planners)))
     times = np.transpose(times)
 
diff --git a/tools/pyproject.toml b/tools/pyproject.toml
new file mode 100644
index 00000000000..b4a54ff7b49
--- /dev/null
+++ b/tools/pyproject.toml
@@ -0,0 +1,50 @@
+[tool.codespell]
+builtin = "clear,rare,informal,usage,code,names"
+check-filenames = true
+check-hidden = true
+ignore-words = "tools/.codespell_ignore_words"
+interactive = 0
+quiet = 34
+skip="*.pgm,./build/*,./install/*,./log*,./.venv/*,./.git*,*.toml"
+uri-ignore-words-list = "segue"
+write-changes = true
+[tool.isort]
+profile = "google"
+force_single_line = false
+line_length = 99
+
+[tool.mypy]
+explicit_package_bases = true
+strict = true
+
+[[tool.mypy.overrides]]
+module = [
+    "matplotlib.*",
+    "rtree.*",
+    "launch.*",
+    "ament_index_python.*",
+    "nav2_common.*",
+    "nav2_msgs.*",
+    "nav2_simple_commander.*",
+    "launch_testing.*",
+    "action_msgs.*",
+    "geometry_msgs.*",
+    "sensor_msgs.*",
+    "tf2_ros.*",
+    "nav_msgs.*",
+    "rosgraph_msgs.*",
+    "tf_transformations.*",
+    "ament_pep257.*",
+    "ament_flake8.*",
+    "ament_copyright.*",
+    "builtin_interfaces.*",
+    "lifecycle_msgs.*",
+    "geographic_msgs.*",
+    "rcl_interfaces.*",
+    "std_msgs.*",
+    "zmq.*",
+    "graphviz.*",
+    "transforms3d.*",
+]
+
+ignore_missing_imports = true
diff --git a/tools/skip_keys.txt b/tools/skip_keys.txt
index 8285089c341..262f3239dac 100644
--- a/tools/skip_keys.txt
+++ b/tools/skip_keys.txt
@@ -4,4 +4,4 @@ fastrtps
 libopensplice69
 rti-connext-dds-5.3.1
 slam_toolbox
-urdfdom_headers
\ No newline at end of file
+urdfdom_headers
diff --git a/tools/smoother_benchmarking/README.md b/tools/smoother_benchmarking/README.md
index d1f870094b3..0be09114eff 100644
--- a/tools/smoother_benchmarking/README.md
+++ b/tools/smoother_benchmarking/README.md
@@ -52,22 +52,22 @@ index c7a90bcb..6f93edbf 100644
 +++ b/nav2_planner/src/planner_server.cpp
 @@ -381,7 +381,10 @@ void PlannerServer::computePlanThroughPoses()
        }
- 
+
        // Get plan from start -> goal
 +      auto planning_start = steady_clock_.now();
        nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id);
 +      auto planning_duration = steady_clock_.now() - planning_start;
 +      result->planning_time = planning_duration;
- 
+
        if (!validatePath<ActionThroughPoses>(curr_goal, curr_path, goal->planner_id)) {
          throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path");
 @@ -398,7 +401,7 @@ void PlannerServer::computePlanThroughPoses()
      publishPlan(result->path);
- 
+
      auto cycle_duration = steady_clock_.now() - start_time;
 -    result->planning_time = cycle_duration;
 +    // result->planning_time = cycle_duration;
- 
+
      if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) {
        RCLCPP_WARN(
 diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp
@@ -75,13 +75,13 @@ index ada1f664..610e9512 100644
 --- a/nav2_smoother/src/nav2_smoother.cpp
 +++ b/nav2_smoother/src/nav2_smoother.cpp
 @@ -253,8 +253,6 @@ bool SmootherServer::findSmootherId(
- 
+
  void SmootherServer::smoothPlan()
  {
 -  auto start_time = steady_clock_.now();
 -
    RCLCPP_INFO(get_logger(), "Received a path to smooth.");
- 
+
    auto result = std::make_shared<Action::Result>();
 @@ -271,6 +269,8 @@ void SmootherServer::smoothPlan()
      // Perform smoothing
diff --git a/tools/smoother_benchmarking/metrics.py b/tools/smoother_benchmarking/metrics.py
index fa01be9754f..6eea9c36c29 100644
--- a/tools/smoother_benchmarking/metrics.py
+++ b/tools/smoother_benchmarking/metrics.py
@@ -18,18 +18,22 @@
 import os
 import pickle
 from random import randint, seed, uniform
+from typing import Optional
 
 from geometry_msgs.msg import PoseStamped
 from nav2_simple_commander.robot_navigator import BasicNavigator
 import numpy as np
+from numpy.typing import NDArray
 import rclpy
+from rclpy.time import Time
 from transforms3d.euler import euler2quat
 
-
 # Note: Map origin is assumed to be (0,0)
 
 
-def getPlannerResults(navigator, initial_pose, goal_pose, planner):
+def getPlannerResults(
+        navigator: BasicNavigator, initial_pose: PoseStamped,
+        goal_pose: PoseStamped, planner: str) -> PoseStamped:
     result = navigator._getPathImpl(initial_pose, goal_pose, planner, use_start=True)
     if result is None or result.error_code != 0:
         print(planner, 'planner failed to produce the path')
@@ -37,7 +41,9 @@ def getPlannerResults(navigator, initial_pose, goal_pose, planner):
     return result
 
 
-def getSmootherResults(navigator, path, smoothers):
+def getSmootherResults(
+        navigator: BasicNavigator, path: PoseStamped,
+        smoothers: list[str]) -> Optional[list[PoseStamped]]:
     smoothed_results = []
     for smoother in smoothers:
         smoothed_result = navigator._smoothPathImpl(path, smoother)
@@ -49,7 +55,8 @@ def getSmootherResults(navigator, path, smoothers):
     return smoothed_results
 
 
-def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res):
+def getRandomStart(costmap: NDArray[np.float32], max_cost: int,
+                   side_buffer: int, time_stamp: Time, res: float) -> PoseStamped:
     start = PoseStamped()
     start.header.frame_id = 'map'
     start.header.stamp = time_stamp
@@ -71,7 +78,10 @@ def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res):
     return start
 
 
-def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res):
+def getRandomGoal(
+        costmap: NDArray[np.float32], start: PoseStamped,
+        max_cost: int, side_buffer: int,
+        time_stamp: Time, res: float) -> PoseStamped:
     goal = PoseStamped()
     goal.header.frame_id = 'map'
     goal.header.stamp = time_stamp
@@ -101,7 +111,7 @@ def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res):
     return goal
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()
diff --git a/tools/smoother_benchmarking/process_data.py b/tools/smoother_benchmarking/process_data.py
index 9aab2c9f997..1ec3ede9beb 100644
--- a/tools/smoother_benchmarking/process_data.py
+++ b/tools/smoother_benchmarking/process_data.py
@@ -20,12 +20,16 @@
 import pickle
 
 import matplotlib.pylab as plt
+from nav2_msgs.action import ComputePathToPose, SmoothPath
+from nav2_msgs.msg import Costmap
+from nav_msgs.msg import Path
 import numpy as np
+from numpy.typing import NDArray
 import seaborn as sns
 from tabulate import tabulate
 
 
-def getPaths(results):
+def getPaths(results: list[ComputePathToPose.Result | SmoothPath.Result]) -> list[Path]:
     paths = []
     for i in range(len(results)):
         if (i % 2) == 0:
@@ -38,7 +42,7 @@ def getPaths(results):
     return paths
 
 
-def getTimes(results):
+def getTimes(results: list[ComputePathToPose.Result | SmoothPath.Result]) -> list[float]:
     times = []
     for i in range(len(results)):
         if (i % 2) == 0:
@@ -56,7 +60,7 @@ def getTimes(results):
     return times
 
 
-def getMapCoordsFromPaths(paths, resolution):
+def getMapCoordsFromPaths(paths: list[Path], resolution: float) -> list[list[float]]:
     coords = []
     for path in paths:
         x = []
@@ -69,8 +73,8 @@ def getMapCoordsFromPaths(paths, resolution):
     return coords
 
 
-def getPathLength(path):
-    path_length = 0
+def getPathLength(path: Path) -> float:
+    path_length = 0.0
     x_prev = path.poses[0].pose.position.x
     y_prev = path.poses[0].pose.position.y
     for i in range(1, len(path.poses)):
@@ -85,14 +89,16 @@ def getPathLength(path):
 
 
 # Path smoothness calculations
-def getSmoothness(pt_prev, pt, pt_next):
+def getSmoothness(
+        pt_prev: NDArray[np.float64], pt: NDArray[np.float64],
+        pt_next: NDArray[np.float64]) -> float:
     d1 = pt - pt_prev
     d2 = pt_next - pt
     delta = d2 - d1
-    return np.linalg.norm(delta)
+    return float(np.linalg.norm(delta))
 
 
-def getPathSmoothnesses(paths):
+def getPathSmoothnesses(paths: list[Path]) -> list[float]:
     smoothnesses = []
     pm0 = np.array([0.0, 0.0])
     pm1 = np.array([0.0, 0.0])
@@ -112,7 +118,8 @@ def getPathSmoothnesses(paths):
 
 
 # Curvature calculations
-def arcCenter(pt_prev, pt, pt_next):
+def arcCenter(pt_prev: NDArray[np.float64], pt: NDArray[np.float64],
+              pt_next: NDArray[np.float64]) -> NDArray[np.float64]:
     cusp_thresh = -0.7
 
     d1 = pt - pt_prev
@@ -129,7 +136,7 @@ def arcCenter(pt_prev, pt, pt_next):
 
     det = d1[0] * d2[1] - d1[1] * d2[0]
     if abs(det) < 1e-4:  # straight line
-        return (float('inf'), float('inf'))
+        return np.array([float('inf'), float('inf')])
 
     # circle center is at the intersection of mirror axes of the segments:
     # http://paulbourke.net/geometry/circlesphere/
@@ -147,7 +154,7 @@ def arcCenter(pt_prev, pt, pt_next):
     return center
 
 
-def getPathCurvatures(paths):
+def getPathCurvatures(paths: list[Path]) -> list[float]:
     curvatures = []
     pm0 = np.array([0.0, 0.0])
     pm1 = np.array([0.0, 0.0])
@@ -169,7 +176,7 @@ def getPathCurvatures(paths):
     return curvatures
 
 
-def plotResults(costmap, paths):
+def plotResults(costmap: Costmap, paths: list[Path]) -> None:
     coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution)
     data = np.asarray(costmap.data)
     data.resize(costmap.metadata.size_y, costmap.metadata.size_x)
@@ -184,12 +191,14 @@ def plotResults(costmap, paths):
     plt.show()
 
 
-def averagePathCost(paths, costmap, num_of_planners):
+def averagePathCost(
+        paths: list[Path], costmap: Costmap,
+        num_of_planners: int) -> list[list[float]]:
     coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution)
     data = np.asarray(costmap.data)
     data.resize(costmap.metadata.size_y, costmap.metadata.size_x)
 
-    average_path_costs = []
+    average_path_costs: list[list[float]] = []
     for i in range(num_of_planners):
         average_path_costs.append([])
 
@@ -204,12 +213,14 @@ def averagePathCost(paths, costmap, num_of_planners):
     return average_path_costs
 
 
-def maxPathCost(paths, costmap, num_of_planners):
+def maxPathCost(
+        paths: list[Path], costmap: Costmap,
+        num_of_planners: int) -> list[list[float]]:
     coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution)
     data = np.asarray(costmap.data)
     data.resize(costmap.metadata.size_y, costmap.metadata.size_x)
 
-    max_path_costs = []
+    max_path_costs: list[list[float]] = []
     for i in range(num_of_planners):
         max_path_costs.append([])
 
@@ -226,7 +237,7 @@ def maxPathCost(paths, costmap, num_of_planners):
     return max_path_costs
 
 
-def main():
+def main() -> None:
     # Read the data
     benchmark_dir = os.getcwd()
     print('Read data')
@@ -244,11 +255,11 @@ def main():
 
     # Paths (planner and smoothers)
     paths = getPaths(results)
-    path_lengths = []
+    path_lengths_list = []
 
     for path in paths:
-        path_lengths.append(getPathLength(path))
-    path_lengths = np.asarray(path_lengths)
+        path_lengths_list.append(getPathLength(path))
+    path_lengths = np.asarray(path_lengths_list)
     total_paths = len(paths)
 
     # [planner, smoothers] path length in a row
@@ -257,8 +268,7 @@ def main():
     path_lengths = path_lengths.transpose()
 
     # Times
-    times = getTimes(results)
-    times = np.asarray(times)
+    times = np.asarray(getTimes(results))
     times.resize((int(total_paths / methods_num), methods_num))
     times = np.transpose(times)
 
@@ -267,14 +277,12 @@ def main():
     max_path_costs = np.asarray(maxPathCost(paths, costmap, methods_num))
 
     # Smoothness
-    smoothnesses = getPathSmoothnesses(paths)
-    smoothnesses = np.asarray(smoothnesses)
+    smoothnesses = np.asarray(getPathSmoothnesses(paths))
     smoothnesses.resize((int(total_paths / methods_num), methods_num))
     smoothnesses = np.transpose(smoothnesses)
 
     # Curvatures
-    curvatures = getPathCurvatures(paths)
-    curvatures = np.asarray(curvatures)
+    curvatures = np.asarray(getPathCurvatures(paths))
     curvatures.resize((int(total_paths / methods_num), methods_num))
     curvatures = np.transpose(curvatures)
 
diff --git a/tools/smoother_benchmarking/smoother_benchmark_bringup.py b/tools/smoother_benchmarking/smoother_benchmark_bringup.py
index c19d1f2f703..86baea19ce5 100644
--- a/tools/smoother_benchmarking/smoother_benchmark_bringup.py
+++ b/tools/smoother_benchmarking/smoother_benchmark_bringup.py
@@ -21,7 +21,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     benchmark_dir = os.getcwd()
     metrics_py = os.path.join(benchmark_dir, 'metrics.py')
diff --git a/tools/underlay.repos b/tools/underlay.repos
index e443ce12489..2384238ee5b 100644
--- a/tools/underlay.repos
+++ b/tools/underlay.repos
@@ -1,8 +1,8 @@
 repositories:
-  # BehaviorTree/BehaviorTree.CPP:
-  #   type: git
-  #   url: https://github.com/BehaviorTree/BehaviorTree.CPP.git
-  #   version: master
+  BehaviorTree/BehaviorTree.CPP:
+    type: git
+    url: https://github.com/BehaviorTree/BehaviorTree.CPP.git
+    version: 4.6.2
   # ros/angles:
   #   type: git
   #   url: https://github.com/ros/angles.git
@@ -18,11 +18,11 @@ repositories:
   # ros/diagnostics:
   #   type: git
   #   url: https://github.com/ros/diagnostics.git
-  #   version: ros2-devel   
+  #   version: ros2-devel
   # ros/geographic_info:
   #   type: git
   #   url: https://github.com/ros-geographic-info/geographic_info.git
-  #   version: ros2      
+  #   version: ros2
   # ompl/ompl:
   #   type: git
   #   url: https://github.com/ompl/ompl.git
@@ -31,12 +31,11 @@ repositories:
   #   type: git
   #   url:  https://github.com/cra-ros-pkg/robot_localization.git
   #   version: ros2
-  ros-navigation/nav2_minimal_turtlebot_simulation:
-    type: git
-    url:  https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git
-    version: 091b5ff12436890a54de6325df3573ae110e912b
+  # ros-navigation/nav2_minimal_turtlebot_simulation:
+  #   type: git
+  #   url:  https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git
+  #  version: 091b5ff12436890a54de6325df3573ae110e912b
   ros/common_interfaces:
     type: git
     url:  https://github.com/ros2/common_interfaces.git
     version: rolling
-
diff --git a/tools/update_readme_table.py b/tools/update_readme_table.py
index b743a988842..94e0daede86 100755
--- a/tools/update_readme_table.py
+++ b/tools/update_readme_table.py
@@ -61,13 +61,19 @@
 # Set which distributions you care about
 Distros = ['humble', 'iron', 'jazzy']
 
-def getSrcPath(package, prefix, OS):
+
+def getSrcPath(package: str, prefix: str, OS: str) -> str:
     return f'https://build.ros2.org/job/{prefix}src_u{OS[0]}__{package}__ubuntu_{OS}__source/'
 
-def getBinPath(package, prefix, OS):
-    return f'https://build.ros2.org/job/{prefix}bin_u{OS[0]}64__{package}__ubuntu_{OS}_amd64__binary/'
 
-def createPreamble(Distros):
+def getBinPath(package: str, prefix: str, OS: str) -> str:
+    return (
+        f'https://build.ros2.org/job/{prefix}bin_u{OS[0]}64__{package}__ubuntu_{OS}_'
+        'amd64__binary/'
+    )
+
+
+def createPreamble(Distros: list[str]) -> str:
     table = '| Package | '
     for distro in Distros:
         table += distro + ' Source | ' + distro + ' Debian | '
@@ -77,7 +83,8 @@ def createPreamble(Distros):
         table += ' :---: | :---: |'
     return table
 
-def main():
+
+def main() -> None:
     header = createPreamble(Distros)
 
     body = ''
@@ -98,13 +105,15 @@ def main():
                 entry += f'[![Build Status]({binURL}badge/icon)]({binURL}) | '
         entry += '\n'
         body += entry
-    
+
     # Special case for Opennav Docking for directory structure of Nav2
     body = body.replace('| opennav_docking |', '| nav2_docking |')
     # Special case for reducing the label length
-    body = body.replace('| nav2_regulated_pure_pursuit_controller |', '| nav2_regulated_pure_pursuit |')
-    
+    body = body.replace('| nav2_regulated_pure_pursuit_controller |',
+                        '| nav2_regulated_pure_pursuit |')
+
     print(header + '\n' + body)
 
+
 if __name__ == '__main__':
     main()