diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile
index fde617772..919452d0c 100644
--- a/.devcontainer/Dockerfile
+++ b/.devcontainer/Dockerfile
@@ -1,4 +1,4 @@
-FROM ghcr.io/ubcsailbot/sailbot_workspace/dev:add-py-deps
+FROM ghcr.io/ubcsailbot/sailbot_workspace/dev:add-py-deps-v2
 
 # Copy configuration files (e.g., .vimrc) from config/ to the container's home directory
 ARG USERNAME=ros
diff --git a/.devcontainer/base-dev/base-dev.Dockerfile b/.devcontainer/base-dev/base-dev.Dockerfile
index 4c49e7b55..ed8a5ecab 100644
--- a/.devcontainer/base-dev/base-dev.Dockerfile
+++ b/.devcontainer/base-dev/base-dev.Dockerfile
@@ -1,4 +1,4 @@
-FROM ghcr.io/ubcsailbot/sailbot_workspace/pre-base:ros_humble-ompl_4c86b2f as base
+FROM ghcr.io/ubcsailbot/sailbot_workspace/pre-base:ros_humble-ompl_4c86b2f-mongo_367-v2 as base
 
 # install base apt dependencies
 ENV DEBIAN_FRONTEND=noninteractive
@@ -75,23 +75,11 @@ RUN apt-get update \
     && apt-get install -y --no-install-recommends \
         mongodb-database-tools \
         mongodb-mongosh \
-        libmongoc-dev \
     && apt-get autoremove -y \
     && apt-get clean -y \
     && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/*
 ENV DEBIAN_FRONTEND=
 
-# setup MongoDB C++ Packages
-ENV DEBIAN_FRONTEND=noninteractive
-# mongo-cxx-driver version must match libmongoc-dev version - see https://mongocxx.org/mongocxx-v3/installation/linux/
-RUN wget https://github.com/mongodb/mongo-cxx-driver/releases/download/r3.6.7/mongo-cxx-driver-r3.6.7.tar.gz \
-    && tar -xzf mongo-cxx-driver-r3.6.7.tar.gz \
-    && cd mongo-cxx-driver-r3.6.7/build \
-    && cmake .. -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr/local \
-    && cmake --build . \
-    && cmake --build . --target  install
-ENV DEBIAN_FRONTEND=
-
 FROM local-base as ros-dev
 
 # From https://github.com/athackst/dockerfiles/blob/32a872348af0ad25ec4a6e6184cb803357acb6ab/ros2/humble.Dockerfile
diff --git a/.devcontainer/pre-base/build-pre-base.sh b/.devcontainer/pre-base/build-pre-base.sh
index 121e458f9..7cdba38e5 100755
--- a/.devcontainer/pre-base/build-pre-base.sh
+++ b/.devcontainer/pre-base/build-pre-base.sh
@@ -2,6 +2,6 @@
 
 docker buildx build . \
     --file pre-base.Dockerfile \
-    --tag ghcr.io/ubcsailbot/sailbot_workspace/pre-base:ros_humble-ompl_4c86b2f \
+    --tag ghcr.io/ubcsailbot/sailbot_workspace/pre-base:ros_humble-ompl_4c86b2f-mongo_367-v2 \
     --platform linux/arm64,linux/amd64 \
     --push
diff --git a/.devcontainer/pre-base/pre-base.Dockerfile b/.devcontainer/pre-base/pre-base.Dockerfile
index b73beb1a1..66c342830 100644
--- a/.devcontainer/pre-base/pre-base.Dockerfile
+++ b/.devcontainer/pre-base/pre-base.Dockerfile
@@ -1,4 +1,15 @@
-FROM ubuntu:22.04 AS ompl-source
+FROM ubuntu:22.04 AS fix-certificates
+
+ENV DEBIAN_FRONTEND=noninteractive
+RUN apt-get update \
+    && apt-get install -y --no-install-recommends \
+        ca-certificates \
+    && apt-get autoremove -y \
+    && apt-get clean -y \
+    && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/*
+ENV DEBIAN_FRONTEND=
+
+FROM fix-certificates AS ompl-source
 
 ENV DEBIAN_FRONTEND=noninteractive
 RUN apt-get update \
@@ -11,7 +22,7 @@ WORKDIR /ompl
 RUN git reset --hard 4c86b2f
 
 # From https://github.com/athackst/dockerfiles/blob/32a872348af0ad25ec4a6e6184cb803357acb6ab/ros2/humble.Dockerfile
-FROM ubuntu:22.04 AS ros-pre-base
+FROM fix-certificates AS ros-pre-base
 
 ENV DEBIAN_FRONTEND=noninteractive
 
@@ -55,7 +66,7 @@ ENV ROS_VERSION=2
 ENV DEBIAN_FRONTEND=
 
 # Based on https://github.com/ompl/ompl/blob/4c86b2fecf7084ae9073bf6a837176d0be169721/scripts/docker/ompl.Dockerfile
-FROM ros-pre-base AS ompl-builder
+FROM fix-certificates AS ompl-builder
 # avoid interactive configuration dialog from tzdata, which gets pulled in
 # as a dependency
 ENV DEBIAN_FRONTEND=noninteractive
@@ -99,6 +110,30 @@ RUN cmake \
     && ninja -j `nproc` \
     && ninja install
 
+FROM fix-certificates AS mongo-cxx-driver-builder
+
+ENV DEBIAN_FRONTEND=noninteractive
+RUN apt-get update \
+    && apt-get install -y --no-install-recommends \
+        build-essential \
+        cmake \
+        git \
+        libmongoc-dev \
+        wget \
+    && apt-get autoremove -y \
+    && apt-get clean -y \
+    && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/*
+
+# setup MongoDB C++ Packages
+# mongo-cxx-driver version must match libmongoc-dev version - see https://mongocxx.org/mongocxx-v3/installation/linux/
+RUN wget https://github.com/mongodb/mongo-cxx-driver/releases/download/r3.6.7/mongo-cxx-driver-r3.6.7.tar.gz \
+    && tar -xzf mongo-cxx-driver-r3.6.7.tar.gz \
+    && cd mongo-cxx-driver-r3.6.7/build \
+    && cmake .. -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr/local \
+    && cmake --build . \
+    && cmake --build . --target  install
+ENV DEBIAN_FRONTEND=
+
 FROM ros-pre-base as pre-base
 LABEL org.opencontainers.image.source = "https://github.com/UBCSailbot/sailbot_workspace"
 
@@ -115,6 +150,7 @@ RUN apt-get update \
         libboost-system-dev \
         libeigen3-dev \
         libflann-dev \
+        libmongoc-dev \
         libode-dev \
         libtriangle-dev \
         ninja-build \
@@ -129,3 +165,4 @@ RUN apt-get update \
 ENV DEBIAN_FRONTEND=
 
 COPY --from=ompl-builder /usr /usr
+COPY --from=mongo-cxx-driver-builder /usr/local /usr/local
diff --git a/build.sh b/build.sh
index c6165525c..ab28ca2f8 100755
--- a/build.sh
+++ b/build.sh
@@ -41,6 +41,9 @@ else
     STATIC_ANALYSIS="ON"
     UNIT_TEST="ON"
 fi
+if [[ $UNIT_TEST = "ON" ]]; then
+    BUILD_TYPE="Debug"
+fi
 
 # Build ROS packages in src directory
 colcon build \