diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml
deleted file mode 100644
index 2174a413..00000000
--- a/.github/workflows/industrial_ci_action.yml
+++ /dev/null
@@ -1,27 +0,0 @@
-name: Industrial-CI
-on: [push, pull_request] # on all pushes and PRs
-
-jobs:
-  CI:
-    strategy:
-      matrix:
-        env:
-          - {ROS_DISTRO: foxy}
-          - {ROS_DISTRO: galactic}
-          - {ROS_DISTRO: rolling}
-          - {ROS_DISTRO: foxy, PRERELEASE: true}
-          - {ROS_DISTRO: galactic, PRERELEASE: true}
-          - {ROS_DISTRO: rolling, PRERELEASE: true}
-    env:
-      CCACHE_DIR: /github/home/.ccache             # Enable ccache
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v2
-      # This step will fetch/store the directory used by ccache before/after the ci run
-      - uses: actions/cache@v2
-        with:
-          path: ${{ env.CCACHE_DIR }}
-          key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}
-      # Run industrial_ci
-      - uses: 'ros-industrial/industrial_ci@master'
-        env: ${{ matrix.env }}
diff --git a/.github/workflows/industrial_ci_build_and_test.yml b/.github/workflows/industrial_ci_build_and_test.yml
new file mode 100644
index 00000000..33d06315
--- /dev/null
+++ b/.github/workflows/industrial_ci_build_and_test.yml
@@ -0,0 +1,25 @@
+name: Industrial-CI
+on: [push, pull_request] # on all pushes and PRs
+
+jobs:
+  CI:
+    strategy:
+      matrix:
+        env:
+          - {ROS_DISTRO: rolling}
+          - {ROS_DISTRO: rolling, PRERELEASE: true}
+          - {ROS_DISTRO: humble, PRERELEASE: true}
+          - {ROS_DISTRO: iron, PRERELEASE: true}
+    # env:
+    #   CCACHE_DIR: /github/home/.ccache             # Enable ccache
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v3
+      # # This step will fetch/store the directory used by ccache before/after the ci run
+      # - uses: actions/cache@v3
+      #   with:
+      #     path: ${{ env.CCACHE_DIR }}
+      #     key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}
+      # Run industrial_ci
+      - uses: 'ros-industrial/industrial_ci@master'
+        env: ${{ matrix.env }}
diff --git a/.github/workflows/build_and_test.yml b/.github/workflows/ros2_ci_build_and_test.yml
similarity index 57%
rename from .github/workflows/build_and_test.yml
rename to .github/workflows/ros2_ci_build_and_test.yml
index 94d6390f..00284923 100644
--- a/.github/workflows/build_and_test.yml
+++ b/.github/workflows/ros2_ci_build_and_test.yml
@@ -12,27 +12,27 @@ jobs:
       fail-fast: false
       matrix:
         ros_distribution:
-          - foxy
-          - galactic
+          - humble
+          - iron
           - rolling
         include:
-          # Foxy Fitzroy (June 2020 - May 2023)
-          - docker_image: rostooling/setup-ros-docker:ubuntu-focal-ros-foxy-ros-base-latest
-            ros_distribution: foxy
+          # Humble Hawksbill (May 2022 - May 2027)
+          - os: ubuntu-22.04
+            ros_distribution: humble
             ros_version: 2
-          # Galactic Geochelone (May 2021 - November 2022)
-          - docker_image: rostooling/setup-ros-docker:ubuntu-focal-ros-galactic-ros-base-latest
-            ros_distribution: galactic
+          # Iron Irwini (May 2023 - November 2024)
+          - os: ubuntu-22.04
+            ros_distribution: iron
             ros_version: 2
           # Rolling Ridley  (June 2020 - Present)
-          - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest
+          - os: ubuntu-22.04
             ros_distribution: rolling
             ros_version: 2
-    container:
-      image: ${{ matrix.docker_image }}
+    runs-on: ${{ matrix.os }}
+    continue-on-error: ${{ matrix.ros_distribution == 'rolling' }}
     steps:
       - name: Checkout
-        uses: actions/checkout@v2
+        uses: actions/checkout@v4
         with:
           fetch-depth: 2
 
@@ -42,7 +42,7 @@ jobs:
           echo ::set-output name=package_list::$(colcon list --names-only)
 
       - name: build and test
-        uses: ros-tooling/action-ros-ci@v0.2
+        uses: ros-tooling/action-ros-ci@v0.3.6
         with:
           package-name: ${{ steps.list_packages.outputs.package_list }}
           target-ros2-distro: ${{ matrix.ros_distribution }}
diff --git a/octomap_server/src/octomap_server.cpp b/octomap_server/src/octomap_server.cpp
index fb85dcb3..0ad3e080 100644
--- a/octomap_server/src/octomap_server.cpp
+++ b/octomap_server/src/octomap_server.cpp
@@ -28,15 +28,15 @@
 
 #include <octomap_server/octomap_server.hpp>
 
-#include <tf2_eigen/tf2_eigen.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-
 #include <algorithm>
 #include <limits>
 #include <memory>
 #include <string>
 #include <vector>
 
+#include <tf2_eigen/tf2_eigen.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
 namespace
 {
 template<typename T>