diff --git a/.github/workflows/main.yaml b/.github/workflows/main.yaml
index 909e36d..4c11309 100644
--- a/.github/workflows/main.yaml
+++ b/.github/workflows/main.yaml
@@ -11,7 +11,7 @@ jobs:
     runs-on: ubuntu-latest
     strategy:
       matrix:
-        ros2_distro: [foxy, galactic]
+        ros2_distro: [galactic]
     steps:
 
       - name: Checkout octomap_server2
diff --git a/.github/workflows/tii-octomap-server2.yaml b/.github/workflows/tii-octomap-server2.yaml
index 6c7f7d8..fc193f3 100644
--- a/.github/workflows/tii-octomap-server2.yaml
+++ b/.github/workflows/tii-octomap-server2.yaml
@@ -38,7 +38,7 @@ jobs:
           tags: localhost:5000/tiiuae/octomap_server2:build_env
           build-args: |
             PACKAGE_NAME=octomap_server2
-            ROS_DISTRO=foxy
+            ROS_DISTRO=galactic
 
       - name: Docker meta
         id: meta
@@ -69,5 +69,5 @@ jobs:
           labels: ${{ steps.meta.outputs.labels }}
           build-args: |
             PACKAGE_NAME=octomap_server2
-            ROS_DISTRO=foxy
+            ROS_DISTRO=galactic
             FROM_IMAGE=localhost:5000/tiiuae/octomap_server2:build_env
diff --git a/Dockerfile b/Dockerfile
index 2ca4009..5cf5514 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -27,5 +27,7 @@ RUN sed --in-place \
         chmod +x /$PACKAGE_NAME/entrypoint.sh
 
 ENV PACKAGE_NAME $PACKAGE_NAME
+ENV RMW_IMPLEMENTATION rmw_fastrtps_cpp
+
 WORKDIR /$PACKAGE_NAME
 ENTRYPOINT "/"$PACKAGE_NAME"/entrypoint.sh"
diff --git a/Dockerfile.build_env b/Dockerfile.build_env
index b422ac8..e2093b6 100644
--- a/Dockerfile.build_env
+++ b/Dockerfile.build_env
@@ -15,6 +15,7 @@ RUN apt-get update -y && apt-get install -y --no-install-recommends \
     fakeroot \
     dh-make \
     libboost-dev \
+    ros-${ROS_DISTRO}-rmw-fastrtps-cpp \
     && rm -rf /var/lib/apt/lists/*
 
 RUN groupadd -g $GID builder && \
diff --git a/README.md b/README.md
index b31a30e..601664e 100644
--- a/README.md
+++ b/README.md
@@ -3,7 +3,7 @@ Port of the ROS1 [octomap server](https://github.com/OctoMap/octomap_mapping) fo
 #### Installation
 1.  Firstly make sure you have [octomap](https://github.com/OctoMap/octomap.git) installed on your system 
   ```bash
-  sudo apt-get install ros-foxy-octomap ros-foxy-octomap-msgs
+  sudo apt-get install ros-galactic-octomap ros-galactic-octomap-msgs
   ```
 
 2.  Next, clone this ros package to the appropriate ros2 workspace
diff --git a/build.sh b/build.sh
index 818bfa2..af67557 100755
--- a/build.sh
+++ b/build.sh
@@ -10,7 +10,7 @@ git_version_string=${3:-$(git log --date=format:%Y%m%d --pretty=~git%cd.%h -n 1)
 
 build_number=${GITHUB_RUN_NUMBER:=0}
 
-ros_distro=${ROS_DISTRO:=foxy}
+ros_distro=${ROS_DISTRO:=galactic}
 
 iname=${PACKAGE_NAME:=octomap_server2}
 
diff --git a/entrypoint.sh b/entrypoint.sh
index 074f6bb..bed5a56 100644
--- a/entrypoint.sh
+++ b/entrypoint.sh
@@ -1,4 +1,4 @@
 #!/bin/bash
 
-source /opt/ros/foxy/setup.bash
+source /opt/ros/galactic/setup.bash
 ros2 launch octomap_server2 octomap_server.py
diff --git a/tasks.py b/tasks.py
index 7eac519..3247af1 100644
--- a/tasks.py
+++ b/tasks.py
@@ -50,7 +50,7 @@ def clone(c):
           'pull': "always attempt to pull a newer version of the image",
           'ros_distro': "ROS distro to use (Available [foxy, galactic])"}
 )
-def build_env(c, nocache=False, pull=False, ros_distro="foxy", image_name=MODULE_NAME):
+def build_env(c, nocache=False, pull=False, ros_distro="galactic", image_name=MODULE_NAME):
     """
     Create Docker build environment.
     """
@@ -90,7 +90,7 @@ def clean(c, reallyclean=False):
     help={'out_dir': "output directory for the generated deb files",
           'ros_distro': "ROS distro to use (Available [foxy, galactic])"}
 )
-def create_deb_package(c, out_dir="../bin/", ros_distro="foxy", image_name=MODULE_NAME):
+def create_deb_package(c, out_dir="../bin/", ros_distro="galactic", image_name=MODULE_NAME):
     """
     Build debian package
     """
@@ -103,7 +103,7 @@ def create_deb_package(c, out_dir="../bin/", ros_distro="foxy", image_name=MODUL
           'ros_distro': "ROS distro to use (Available [foxy, galactic])",
           'image_name': "name of output docker image"}
 )
-def build_docker(c, nocache=False, pull=False, ros_distro="foxy", image_name=MODULE_NAME):
+def build_docker(c, nocache=False, pull=False, ros_distro="galactic", image_name=MODULE_NAME):
     """
     Build Docker image of this component
     """