diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile
index 0a75886..f8105a9 100644
--- a/.devcontainer/Dockerfile
+++ b/.devcontainer/Dockerfile
@@ -3,7 +3,11 @@
 ARG BASE_IMAGE
 FROM $BASE_IMAGE
 
-
+RUN apt-get update && export DEBIAN_FRONTEND=noninteractive \
+    && apt-get -y install --no-install-recommends \
+    x11-apps \
+    x11-utils \
+    mesa-utils
 
 ARG VNC_RESOLUTION
 ENV VNC_RESOLUTION=$VNC_RESOLUTION
diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json
index 72a5c63..eb7b8e7 100644
--- a/.devcontainer/devcontainer.json
+++ b/.devcontainer/devcontainer.json
@@ -9,9 +9,40 @@
 		}
 	},
 	"runArgs": [
+		// Uncomment it if you have problems with the auto X11 forwarding
+		// "--volume=/tmp/.X11-unix/X0:/tmp/.X11-unix/X0", 
 		"--network=host"
-	]
-	// "runArgs": [
-	// 	"--volume=/tmp/.X11-unix/X0:/tmp/.X11-unix/X0",
-	// ]
+	],
+	"containerEnv": {
+		"SMB_RAW_REPO_URL": "https://raw.githubusercontent.com/ETHZ-RobotX/SuperMegaBot/fix/remove_private_repos",
+		"DISPLAY": "${localEnv:DISPLAY}"
+	},
+	"postCreateCommand": "bash ./.devcontainer/post-create.sh",
+	"customizations": {
+		"vscode": {
+			"settings": {
+				"git.autoRepositoryDetection": true,
+				"git.repositoryScanMaxDepth": 5,
+				"git.ignoredRepositories": [
+					"src/lib/ocs2",
+					"src/lib/gtsam_catkin",
+					"src/misc/minkindr",
+					"src/misc/catkin_simplWe",
+					"src/misc/minkindr_ros",
+					"src/misc/eigen_checks",
+					"src/misc/glog_catkin",
+					"src/misc/gflags_catkin",
+					"src/misc/mav_comm",
+					"src/misc/eigen_catkin",
+					"src/misc/yaml_cpp_catkin"
+				]
+			},
+			"extensions": [
+				"ms-iot.vscode-ros",
+				"redhat.vscode-xml",
+				"ms-vscode.cpptools",
+				"ms-vscode.cpptools-themes"
+			]
+		}
+	}
 }
\ No newline at end of file
diff --git a/.devcontainer/post-create.sh b/.devcontainer/post-create.sh
new file mode 100644
index 0000000..2d00bdd
--- /dev/null
+++ b/.devcontainer/post-create.sh
@@ -0,0 +1,10 @@
+#!/bin/bash
+set -e
+
+ROOT=$(dirname "$(dirname "$(readlink -f $0)")")
+# Clone the repository
+vcs import --input "${SMB_RAW_REPO_URL}/smb.repos" --recursive --skip-existing "${ROOT}/src"
+
+# setup catkin workspace
+catkin init --workspace "${ROOT}/src" 
+catkin config --workspace "${ROOT}/src" --extend /opt/ros/noetic -DCMAKE_BUILD_TYPE=Release
diff --git a/.github/.devcontainer/Dockerfile b/.github/.devcontainer/Dockerfile
index f4a3e97..f5e4caa 100755
--- a/.github/.devcontainer/Dockerfile
+++ b/.github/.devcontainer/Dockerfile
@@ -1,12 +1,12 @@
 # syntax=docker/dockerfile:1
 
 ARG OVERLAY_WS=/opt/ros/overlay_ws
+ARG SMB_RAW_REPO_URL=https://raw.githubusercontent.com/ETHZ-RobotX/SuperMegaBot/master
 
 # multi-stage build for caching
 FROM osrf/ros:noetic-desktop-full AS cacher
 
 ARG OVERLAY_WS
-ARG SMB_RAW_REPO_URL=https://raw.githubusercontent.com/ETHZ-RobotX/SuperMegaBot/master
 
 RUN DEBIAN_FRONTEND=noninteractive apt-get update && apt-get install -y --no-install-recommends \
     python3-vcstool \
@@ -67,16 +67,19 @@ RUN mkdir -p /usr/share/yolo/models && \
 
 # RUN python3 -m pip install -r src/object_detection/requirements.txt
 
-# # Build SMB packages
-# COPY --from=cacher $OVERLAY_WS/src ./src
-# RUN catkin init && \
-#     catkin config --extend /opt/ros/noetic -DCMAKE_BUILD_TYPE=Release && \
-#     catkin build --no-status \
-#     smb_gazebo \
-#     smb_path_planner \
-#     smb_slam
+# Build SMB packages
+COPY --from=cacher $OVERLAY_WS/src ./src
+RUN catkin init && \
+    catkin config --extend /opt/ros/noetic -DCMAKE_BUILD_TYPE=Release && \
+    catkin build --no-status \
+    smb_gazebo \
+    smb_path_planner \
+    smb_slam
 
 # RUN catkin build --no-status object_detection
 
 # # RUN catkin build --no-status smb_msf_graph
 
+ARG SMB_RAW_REPO_URL
+ENV SMB_RAW_REPO_URL=${SMB_RAW_REPO_URL}
+
diff --git a/README.md b/README.md
index 0d78ede..ab852ca 100644
--- a/README.md
+++ b/README.md
@@ -1,5 +1,5 @@
 # Using Docker for Simulation
-[![Open in Dev Containers](https://img.shields.io/static/v1?label=Dev%20Containers&message=Open&color=blue&logo=visualstudiocode)](https://vscode.dev/redirect?url=vscode://ms-vscode-remote.remote-containers/cloneInVolume?url=https://github.com/ETHZ-RobotX/smb_docker)
+[![Open in Dev Containers](https://img.shields.io/static/v1?label=Dev%20Containers&message=Open&color=blue&logo=visualstudiocode)](https://vscode.dev/redirect?url=vscode://ms-vscode-remote.remote-containers/cloneInVolume?url=https://github.com/ETHZ-RobotX/smb_docker/tree/feature/wip_devcontainer)
 
 > To use the SMB Docker, basic knowledge of Docker is needed. Please check [the official website](https://docs.docker.com) to learn how to build, save, reconnect etc.
 
diff --git a/src/README.md b/src/README.md
new file mode 100644
index 0000000..17e0f0d
--- /dev/null
+++ b/src/README.md
@@ -0,0 +1 @@
+# Notes