diff --git a/.circleci/config.yml b/.circleci/config.yml
index e0a3aab16..c55813a9f 100644
--- a/.circleci/config.yml
+++ b/.circleci/config.yml
@@ -1,8 +1,13 @@
 version: 2
+
 jobs:
   build_and_test:
     docker:
       - image: ros:foxy
+    environment:
+      ROS_WS: "/opt/ros"
+      UNDERLAY_WS: "/opt/underlay_ws"
+      OVERLAY_WS: "/opt/overlay_ws"
     steps:
       - checkout
       - run:
@@ -10,19 +15,26 @@ jobs:
           command: |
             apt update -qq && apt install -y build-essential cmake python3-colcon-common-extensions python3-rosdep libeigen3-dev
             apt upgrade -y
-            source `find /opt/ros -maxdepth 2 -name local_setup.bash | sort | head -1`
-            mkdir -p src/grid_map && mv `find -maxdepth 1 -not -name . -not -name src` src/grid_map/
+            source `find $ROS_WS -maxdepth 2 -name local_setup.bash | sort | head -1`
+            mkdir -p $UNDERLAY_WS/src
+            cp $OVERLAY_WS/src/grid_map/tools/ros2_dependencies.repos \
+              $UNDERLAY_WS/ros2_dependencies.repos
+            cd $UNDERLAY_WS && vcs import src < ros2_dependencies.repos
+            colcon build --symlink-install --packages-up-to nav2_costmap_2d
+            source $UNDERLAY_WS/install/setup.bash
+            cd $OVERLAY_WS
+            mkdir -p $OVERLAY_WS/src/grid_map && mv `find -maxdepth 1 -not -name . -not -name src` $OVERLAY_WS/src/grid_map/
             rosdep update
             rosdep install -y --ignore-src --from-paths src
       - run:
           name: Debug Build
           command: |
-            source `find /opt/ros -maxdepth 2 -name local_setup.bash | sort | head -1`
+            source `find $ROS_WS -maxdepth 2 -name local_setup.bash | sort | head -1`
             colcon build --parallel-workers 1
       - run:
           name: Run Tests
           command: |
-            source `find /opt/ros -maxdepth 2 -name local_setup.bash | sort | head -1`
+            source `find $ROS_WS -maxdepth 2 -name local_setup.bash | sort | head -1`
             colcon test --parallel-workers 1
             colcon test-result --verbose
 workflows:
diff --git a/tools/ros2_dependencies.repos b/tools/ros2_dependencies.repos
new file mode 100644
index 000000000..6f6448bc0
--- /dev/null
+++ b/tools/ros2_dependencies.repos
@@ -0,0 +1,5 @@
+repositories:
+  ros-planning/navigation2:
+    type: git
+    url: https://github.com/ros-planning/navigation2.git
+    version: master