diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile
deleted file mode 100644
index 64887d8..0000000
--- a/.devcontainer/Dockerfile
+++ /dev/null
@@ -1,31 +0,0 @@
-FROM ros:humble
-ENV DEBIAN_FRONTEND=noninteractive
-SHELL ["/bin/bash", "-c"]
-RUN apt-get update && apt-get install -y -qq --no-install-recommends \ 
-                                      libglvnd-dev \
-                                      libgl1-mesa-dev \
-                                      libegl1-mesa-dev \
-                                      libgles2-mesa-dev \
-                                      libxext6 \
-                                      libx11-6 \
-                                      freeglut3-dev \
-                                      git \
-                                      python3-pip \ 
-                                      ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \
-                                      libyaml-cpp-dev \
-                                      ros-humble-xacro \
-                                      libboost-all-dev\
-                                      build-essential \
-                                      cmake \
-                                      && rm -rf /var/lib/apt/lists/*
-
-
-RUN pip3 install mujoco pin matplotlib
-
-RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /unitree_ros2/cyclonedds_ws/src && \
-git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\
-cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build
-
-# Env vars for the nvidia-container-runtime.
-ENV NVIDIA_VISIBLE_DEVICES all
-ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute
\ No newline at end of file
diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json
index 14e7378..02bdfc8 100644
--- a/.devcontainer/devcontainer.json
+++ b/.devcontainer/devcontainer.json
@@ -1,13 +1,65 @@
-// https://containers.dev/implementors/json_reference/
 {
-  "name": "go2py", 
-  "dockerComposeFile": "docker-compose.yaml",
-  "workspaceFolder": "/home/Go2Py",
-  "service": "go2py-dev",
-  "remoteUser": "root", 
-  "customizations": {
-    "vscode": {
-      "extensions": ["dbaeumer.vscode-eslint"]
+    "name": "go2-devcontainer",
+    "build": {
+        "context": "..",
+        "dockerfile": "../docker/build.sh"
+    },
+    "containerEnv": {
+        "DISPLAY": "unix:0",
+        "ROS_LOCALHOST_ONLY": "0",
+        "ROS_AUTOMATIC_DISCOVERY_RANGE": "SUBNET",
+        "ROS_DOMAIN_ID": "0",
+        "SHELL": "/bin/bash"
+    },
+    "runArgs": [
+        "--privileged",
+        "--gpus",
+        "all",
+        "-v",
+        "/dev:/dev",
+        "--net=host",
+        "-e",
+        "DISPLAY=${env:DISPLAY}",
+        "--ulimit=core=-1"
+    ],
+    "mounts": [
+        "source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached",
+        "source=/dev/dri,target=/dev/dri,type=bind,consistency=cached",
+        "source=${localEnv:HOME}/go2-devcontainer/logging/,target=/workspace/logging,type=bind,consistency=cached",
+        "source=${localEnv:HOME}/go2-devcontainer/.bash_history,target=/home/dev/.bash_history,type=bind,consistency=cached",
+    ],
+    "workspaceMount": "source=${localWorkspaceFolder},target=/workspace/go2-devcontainer/src/go2-devcontainer,type=bind",
+    "workspaceFolder": "/workspace/go2-devcontainer/src",
+    "customizations": {
+        "vscode": {
+            "settings": {
+                "extensions.verifySignature": false
+            },
+            "extensions": [
+                "cheshirekow.cmake-format",
+                "llvm-vs-code-extensions.vscode-clangd",
+                "ms-azuretools.vscode-docker",
+                "ms-iot.vscode-ros",
+                "ms-python.black-formatter",
+                "ms-python.flake8",
+                "ms-python.isort",
+                "ms-python.pylint",
+                "ms-python.python",
+                "ms-toolsai.jupyter",
+                "ms-vscode.cmake-tools",
+                "mutantdino.resourcemonitor",
+                "vadimcn.vscode-lldb",
+                "xaver.clang-format"
+            ]
+        }
+    },
+    "forwardPorts": [
+        3000
+    ],
+    "portsAttributes": {
+        "3000": {
+            "label": "Application",
+            "onAutoForward": "notify"
+        }
     }
-  }
 }
\ No newline at end of file
diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml
deleted file mode 100644
index 54d71e4..0000000
--- a/.devcontainer/docker-compose.yaml
+++ /dev/null
@@ -1,17 +0,0 @@
-version: "3.9"
-services:
-  go2py-dev:
-    build: .
-    container_name: go2py-dev
-    network_mode: host
-    privileged: true
-    command: bash
-    volumes:
-      - /tmp/.X11-unix:/tmp/.X11-unix
-      - ..:/home/Go2Py
-    environment:
-      - DISPLAY=${DISPLAY}
-      - QT_X11_NO_MITSHM=1
-    runtime: nvidia
-    stdin_open: true
-    tty: true
\ No newline at end of file
diff --git a/.flake8 b/.flake8
new file mode 100644
index 0000000..bf750d2
--- /dev/null
+++ b/.flake8
@@ -0,0 +1,2 @@
+[flake8]
+max-line-length = 99
\ No newline at end of file
diff --git a/.gitattributes b/.gitattributes
new file mode 100644
index 0000000..43751c6
--- /dev/null
+++ b/.gitattributes
@@ -0,0 +1,10 @@
+*.dae filter=lfs diff=lfs merge=lfs -text
+*.jpeg filter=lfs diff=lfs merge=lfs -text
+*.jpg filter=lfs diff=lfs merge=lfs -text
+*.obj filter=lfs diff=lfs merge=lfs -text
+*.pcd filter=lfs diff=lfs merge=lfs -text
+*.png filter=lfs diff=lfs merge=lfs -text
+*.so filter=lfs diff=lfs merge=lfs -text
+*.so.* filter=lfs diff=lfs merge=lfs -text
+*.stl filter=lfs diff=lfs merge=lfs -text
+*.STL filter=lfs diff=lfs merge=lfs -text
\ No newline at end of file
diff --git a/container_provisioning/README.md b/container_provisioning/README.md
new file mode 100644
index 0000000..1a97807
--- /dev/null
+++ b/container_provisioning/README.md
@@ -0,0 +1,6 @@
+# Container Provisioning
+
+Container provisioning contains scripts to set up a PC ready for deployment.
+
+- System environemnt setup
+- Logging directory creation
diff --git a/container_provisioning/scripts/init-directory.sh b/container_provisioning/scripts/init-directory.sh
new file mode 100755
index 0000000..4079bfa
--- /dev/null
+++ b/container_provisioning/scripts/init-directory.sh
@@ -0,0 +1,23 @@
+#!/bin/bash
+set -e
+
+REQUIRED_DIRS=(
+  "$HOME/go2-devcontainer/logging/coredump"
+  "$HOME/go2-devcontainer/logging/logs"
+  "$HOME/go2-devcontainer/logging/data"
+  "$HOME/go2-devcontainer/logging/plan/data/scene"
+  "$HOME/go2-devcontainer/logging/plan/data/plan_results"
+  "$HOME/go2-devcontainer/models/yolov8"
+)
+
+for dir in "${REQUIRED_DIRS[@]}"; do
+  if [ ! -d "$dir" ]; then
+    echo "Creating directory $dir"
+    mkdir -p $dir
+  fi
+done
+
+if [ ! -f "$HOME/go2-devcontainer/.bash_history" ]; then
+  echo "Creating file $HOME/go2-devcontainer/.bash_history"
+  touch $HOME/go2-devcontainer/.bash_history
+fi
\ No newline at end of file
diff --git a/container_provisioning/scripts/install-docker.sh b/container_provisioning/scripts/install-docker.sh
new file mode 100755
index 0000000..a0e61c9
--- /dev/null
+++ b/container_provisioning/scripts/install-docker.sh
@@ -0,0 +1,48 @@
+#!/bin/bash
+set -e
+
+if ! [ -x "$(command -v curl)" ]; then
+    sudo apt-get install curl
+fi
+
+# If docker is not installed, install it
+if ! [ -x "$(command -v docker)" ]; then
+    echo "Docker is not installed. Installing Docker"
+    curl -fsSL https://get.docker.com -o get-docker.sh
+    sudo sh get-docker.sh
+
+    # Post-installation steps: https://docs.docker.com/engine/install/linux-postinstall/
+    if ! [ $(getent group docker) ]; then
+        sudo groupadd docker
+    fi
+    sudo usermod -aG docker $USER
+
+    echo "=============================================="
+    echo "Docker installed. Please restart your computer."
+    echo "=============================================="
+    exit 1
+fi
+
+# Check if docker is running
+if ! systemctl is-active --quiet docker; then
+    echo "Docker is not running. Please start Docker and run this script again."
+    exit 1
+fi
+
+# If nvidia-container-toolkit is not installed, install it
+# Follow https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html#installing-with-apt
+if ! [ -x "$(command -v nvidia-container-toolkit)" ]; then
+    # Install nvidia-container-toolkit
+    curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg \
+  && curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | \
+    sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \
+    sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list
+    sudo apt-get update
+    sudo apt-get install -y nvidia-container-toolkit
+    # Configure nvidia-container-toolkit
+    sudo nvidia-ctk runtime configure --runtime=docker
+    sudo systemctl restart docker
+    nvidia-ctk runtime configure --runtime=docker --config=$HOME/.config/docker/daemon.json
+    systemctl --user restart docker
+    sudo nvidia-ctk config --set nvidia-container-cli.no-cgroups --in-place
+fi
\ No newline at end of file
diff --git a/container_provisioning/scripts/install-nvidia-driver.sh b/container_provisioning/scripts/install-nvidia-driver.sh
new file mode 100755
index 0000000..951ce5d
--- /dev/null
+++ b/container_provisioning/scripts/install-nvidia-driver.sh
@@ -0,0 +1,24 @@
+#!/bin/bash
+set -e
+
+if ! command -v nvidia-smi &> /dev/null
+then
+    echo "Installing NVIDIA driver"
+else
+    echo "NVIDIA driver is already installed"
+    exit 0
+fi
+
+# Prompt user to confirm to install NVIDIA driver 535
+NVIDIA_DRIVER_VERSION=535
+while true; do
+    read -p "Do you want to install NVIDIA driver ${NVIDIA_DRIVER_VERSION}? (y/n)" yn
+    case $yn in
+        [Yy]* ) break;;
+        [Nn]* ) echo "Please install NVIDIA driver manually!"; exit 1;;
+        * ) echo "Please answer yes or no.";;
+    esac
+done
+
+sudo apt-get update
+sudo apt-get install -y nvidia-driver-${NVIDIA_DRIVER_VERSION}
\ No newline at end of file
diff --git a/container_provisioning/scripts/install-tools.sh b/container_provisioning/scripts/install-tools.sh
new file mode 100755
index 0000000..b0085d7
--- /dev/null
+++ b/container_provisioning/scripts/install-tools.sh
@@ -0,0 +1,34 @@
+#!/bin/bash
+set -e
+
+if ! command -v git-lfs &> /dev/null
+then
+    echo "Installing git-lfs"
+    sudo apt-get update
+    sudo apt-get install \
+        git-lfs
+    echo "=============================================="
+    echo "git-lfs is installed. Please remove cloned go2-devcontainer and re-clone it."
+    echo "=============================================="
+    exit 1
+else
+    echo "git-lfs is already installed"
+fi
+
+# # If foxglove-studio is not installed, install it
+# if ! command -v foxglove-studio &> /dev/null
+# then
+#     echo "Installing foxglove-studio"
+#     sudo snap install foxglove-studio
+# else
+#     echo "foxglove-studio is already installed"
+# fi
+
+# If VSCode is not installed, install it
+if ! command -v code &> /dev/null
+then
+    echo "Installing VSCode"
+    sudo snap install code --classic
+else
+    echo "VSCode is already installed"
+fi
diff --git a/container_provisioning/system-setup.sh b/container_provisioning/system-setup.sh
new file mode 100755
index 0000000..4e2538b
--- /dev/null
+++ b/container_provisioning/system-setup.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+set -e
+
+SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
+${SCRIPT_DIR}/scripts/install-tools.sh
+${SCRIPT_DIR}/scripts/install-nvidia-driver.sh
+${SCRIPT_DIR}/scripts/install-docker.sh
+${SCRIPT_DIR}/scripts/init-directory.sh
\ No newline at end of file
diff --git a/docker/Dockerfile.x86 b/docker/Dockerfile.x86
new file mode 100644
index 0000000..d7e3b95
--- /dev/null
+++ b/docker/Dockerfile.x86
@@ -0,0 +1,65 @@
+FROM nvidia/cuda:12.1.1-devel-ubuntu22.04
+
+SHELL ["/bin/bash", "-c"]
+
+# Install dependencies
+RUN --mount=type=cache,target=/var/cache/apt \
+    --mount=type=bind,source=docker/scripts/install-tools.sh,target=/tmp/install-tools.sh \
+    bash /tmp/install-tools.sh
+RUN --mount=type=cache,target=/var/cache/apt \
+    --mount=type=bind,source=docker/scripts/install-ros2.sh,target=/tmp/install-ros2.sh \
+    bash /tmp/install-ros2.sh
+RUN --mount=type=cache,target=/var/cache/apt \
+    --mount=type=bind,source=docker/scripts/install-clang.sh,target=/tmp/install-clang.sh \
+    bash /tmp/install-clang.sh
+RUN --mount=type=cache,target=/root/.cache/pip \
+    --mount=type=bind,source=docker/scripts/install-python-requirements.sh,target=/tmp/install-python-requirements.sh \
+    bash /tmp/install-python-requirements.sh
+RUN --mount=type=cache,target=/var/cache/apt \
+    --mount=type=bind,source=docker/scripts/install-3rdparty.sh,target=/tmp/install-3rdparty.sh \
+    bash /tmp/install-3rdparty.sh
+
+# Last installation layer as we update ROS dependencies often
+RUN --mount=type=cache,target=/var/cache/apt \
+    --mount=type=bind,source=.,target=/tmp/ros_ws \
+    /tmp/ros_ws/docker/scripts/install-rosdep.sh
+
+# Clean cache
+RUN apt-get autoremove -y \
+    && apt-get clean \
+    && rm -rf /var/lib/apt/lists/*
+
+# Create a non-root user
+ARG USERNAME=dev
+ARG USER_UID=1000
+ARG USER_GID=$USER_UID
+RUN groupadd --gid $USER_GID $USERNAME \
+    && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \
+    && apt-get update \
+    && apt-get install -y sudo \
+    && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \
+    && chmod 0440 /etc/sudoers.d/$USERNAME \
+    && apt-get autoremove -y \
+    && apt-get clean \
+    && rm -rf /var/lib/apt/lists/*
+
+# Create a workspace directory
+RUN mkdir -p /workspace/go2-devcontainer/src/go2-devcontainer \
+    && cd /workspace/go2-devcontainer \
+    && chown -R $USERNAME:$USERNAME /workspace/go2-devcontainer/
+
+RUN --mount=type=bind,source=docker/config/docker.bashrc,target=/tmp/docker.bashrc \
+    cat /tmp/docker.bashrc >> /home/$USERNAME/.bashrc
+
+# Clone unitree_ros2
+RUN --mount=type=cache,target=/var/cache/apt \
+    --mount=type=bind,source=docker/scripts/install-unitree-ros2.sh,target=/tmp/install-unitree-ros2.sh \
+    bash /tmp/install-unitree-ros2.sh
+
+# NVIDIA-related environment variables
+ENV NVIDIA_VISIBLE_DEVICES \
+    ${NVIDIA_VISIBLE_DEVICES:-all}
+ENV NVIDIA_DRIVER_CAPABILITIES \
+    ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics
+
+USER $USERNAME
\ No newline at end of file
diff --git a/docker/build.sh b/docker/build.sh
new file mode 100644
index 0000000..5a96e03
--- /dev/null
+++ b/docker/build.sh
@@ -0,0 +1,12 @@
+#!/bin/bash
+ARCH=$(uname -m)
+if [ "$ARCH" = "x86_64" ]; then
+    DOCKERFILE_PATH="docker/Dockerfile.x86"
+elif [ "$ARCH" = "aarch64" ]; then
+    DOCKERFILE_PATH="docker/Dockerfile.arm64"
+else
+    echo "Unsupported architecture: $ARCH"
+    exit 1
+fi
+
+docker build --file $DOCKERFILE_PATH --tag go2py-container .
diff --git a/docker/config/docker.bashrc b/docker/config/docker.bashrc
new file mode 100644
index 0000000..ec7a490
--- /dev/null
+++ b/docker/config/docker.bashrc
@@ -0,0 +1,149 @@
+##############################################
+## Bash Script
+##############################################
+
+PROMPT_COMMAND='history -a'
+HISTFILE=/home/dev/.bash_history
+LOG_WORKSPACE=/workspace/logging
+ROS_WORKSPACE=/workspace/go2-devcontainer
+
+export CC=clang
+export CXX=clang++
+export MAKEFLAGS="-j 8"
+
+export RCUTILS_COLORIZED_OUTPUT=1
+export ROS_LOG_DIR=/workspace/logging/logs
+export ROS_DATA_DIR=/workspace/logging/data
+export ROS_BAG_MAX_SIZE=1073741824
+export RCUTILS_CONSOLE_OUTPUT_FORMAT="[{severity}] [{time}] [{name}:{line_number}]: {message}"
+
+# unitree_ros2
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces>
+                            <NetworkInterface name="enp118s0" priority="default" multicast="default" />
+                        </Interfaces></General></Domain></CycloneDDS>'
+
+# https://github.com/ament/ament_cmake/issues/382
+PYTHONWARNINGS="ignore:easy_install command is deprecated,ignore:setup.py install is deprecated"
+export PYTHONWARNINGS
+
+echo '/workspace/logging/coredump/core.%e.%p.%h.%t' | sudo tee /proc/sys/kernel/core_pattern >/dev/null
+
+_show_workspace_logging_usage() {
+  # For each dir under logging workspace, show disk usages.
+  echo "==================================================="
+  echo "Disk usage for each directory under $LOG_WORKSPACE:"
+  for dir in $(find $LOG_WORKSPACE -maxdepth 1 -mindepth 1 -type d); do
+    du -sh $dir
+  done
+  echo "Make sure to clean up old logs to save disk space."
+  echo "==================================================="
+}
+
+_update_dependencies() {
+  source /opt/ros/humble/setup.bash
+  sudo apt-get update
+  cd $ROS_WORKSPACE \
+      && rosdep update \
+      && rosdep install --from-paths src --ignore-src -y
+}
+
+_ensure_not_in_workspace_src() {
+  # If current dir ends with "src", prompt user to switch to workspace dir.
+  # colcon build will stupidly build inside src folder.
+  if [[ "$(basename "$(pwd)")" == "src" ]]; then
+    echo "It looks like you're in a 'src' directory. Please switch to ${ROS_WORKSPACE}."
+    return 1
+  fi
+  return 0
+}
+
+COMMON_CMAKE_ARGS="\
+    -DCMAKE_EXPORT_COMPILE_COMMANDS=ON \
+    -GNinja \
+    -DCMAKE_EXE_LINKER_FLAGS=-fuse-ld=lld \
+    -DCMAKE_MODULE_LINKER_FLAGS=-fuse-ld=lld \
+    -DCMAKE_SHARED_LINKER_FLAGS=-fuse-ld=lld
+    "
+COMMON_COLCON_ARGS="--symlink-install --event-handlers console_cohesion+"
+RELEASE_CMAKE_ARGS="$COMMON_CMAKE_ARGS -DCMAKE_BUILD_TYPE=RelWithDebInfo"
+DEBUG_CMAKE_ARGS="$COMMON_CMAKE_ARGS -DCMAKE_BUILD_TYPE=Debug"
+
+_colcon_release_build() {
+  _ensure_not_in_workspace_src || return 1
+
+  # Run 'colcon build' with passed arguments
+  colcon build \
+      ${COMMON_COLCON_ARGS} \
+      --cmake-args $RELEASE_CMAKE_ARGS \
+      "$@"
+}
+
+_colcon_debug_build() {
+  _ensure_not_in_workspace_src || return 1
+
+  # Run 'colcon build' with passed arguments
+  colcon build \
+      ${COMMON_COLCON_ARGS} \
+      --cmake-args $DEBUG_CMAKE_ARGS \
+      "$@"
+}
+
+_colcon_test() {
+  _ensure_not_in_workspace_src || return 1
+
+  # Run 'colcon test' with passed arguments
+  colcon test --event-handlers console_cohesion+ "$@"
+}
+
+_remove_directories() {
+  if [ -d "build" ]; then
+    rm -r build
+  fi
+  if [ -d "install" ]; then
+    rm -r install
+  fi
+  if [ -d "log" ]; then
+    rm -r log
+  fi
+}
+
+colcon_clean() {
+  _ensure_not_in_workspace_src || return 1
+
+  if [ -d "build" ] || [ -d "install" ] || [ -d "log" ]; then
+    while true; do
+      read -p "Do you wish to remove build, install, and log directories? [y/n] " yn
+      case $yn in
+        [Yy]* ) _remove_directories; break;;
+        [Nn]* ) break;;
+        * ) echo "Please answer yes or no.";;
+      esac
+    done
+  fi
+}
+
+# Colcon build aliases
+alias update_dep=_update_dependencies
+alias source_ws='source install/setup.bash'
+alias colcon_build='_colcon_release_build'
+alias colcon_debug_build='_colcon_debug_build'
+alias colcon_build_package='_colcon_release_build --packages-select'
+alias colcon_debug_build_package='_colcon_debug_build --packages-select'
+alias colcon_build_up_to='_colcon_release_build --packages-up-to'
+alias colcon_debug_build_up_to='_colcon_debug_build --packages-up-to'
+alias colcon_test='_colcon_test'
+alias colcon_test_package='_colcon_test --packages-select'
+alias colcon_test_up_to='_colcon_test --packages-up-to'
+
+source /opt/ros/humble/setup.bash
+# Source ROS workspace if it exists
+if [ -f "/workspace/go2-devcontainer/install/setup.bash" ]; then
+    source /workspace/go2-devcontainer/install/setup.bash
+fi
+
+# colcon_cd
+source /usr/share/colcon_cd/function/colcon_cd.sh
+export _colcon_cd_root=/opt/ros/humble/
+
+_show_workspace_logging_usage
\ No newline at end of file
diff --git a/docker/scripts/build-librealsense.sh b/docker/scripts/build-librealsense.sh
new file mode 100644
index 0000000..67fab42
--- /dev/null
+++ b/docker/scripts/build-librealsense.sh
@@ -0,0 +1,188 @@
+#!/bin/bash
+# Builds the Intel Realsense library librealsense on a Jetson Nano Development Kit
+# Copyright (c) 2016-21 Jetsonhacks 
+# MIT License
+
+LIBREALSENSE_DIRECTORY=${HOME}/librealsense
+INSTALL_DIR=$PWD
+NVCC_PATH=/usr/local/cuda/bin/nvcc
+
+USE_CUDA=true
+
+function usage ()
+{
+    echo "Usage: ./build-librealsense.sh [-n | -no_cuda] [-v | -version <version>] [-j | --jobs <number of jobs>] [-h | --help] "
+    echo "-n  | --no_cuda   Build with no CUDA (Defaults to with CUDA)"
+    echo "-v  | --version   Version of librealsense to build 
+                      (defaults to latest release)"
+    echo "-j  | --jobs      Number of concurrent jobs (Default 1 on <= 4GB RAM
+                      #of cores-1 otherwise)"
+    echo "-h  | --help      This message"
+    exit 2
+}
+
+PARSED_ARGUMENTS=$(getopt -a -n build-librealsense.sh -o nv:j:h --longoptions version:,no_cuda,jobs:,help -- "$@" )
+VALID_ARGUMENTS=$?
+
+if [ "$VALID_ARGUMENTS" != "0" ]; then
+   echo ""
+   usage
+fi
+
+eval set -- "$PARSED_ARGUMENTS"
+
+LIBREALSENSE_VERSION=""
+USE_CUDA=true
+NUM_PROCS=""
+
+while :
+do
+   case "$1" in
+      -n | --build_no_cuda) USE_CUDA=false   ; shift ;;
+      -v | --version )      LIBREALSENSE_VERSION="$2" ; shift 2 ;;
+      -j | --jobs)          NUM_PROCS="$2" ; 
+                            shift 2 ;
+                            re_isanum='^[0-9]+$'
+                            if ! [[ $NUM_PROCS =~ $re_isanum ]] ; then
+                              echo "Number of jobs must be a positive, whole number"
+                              usage
+                            else
+                              if [ $NUM_PROCS -eq "0" ]; then
+                                echo "Number of jobs must be a positive, whole number" 
+                              fi
+                            fi ;
+       ;;
+      -h | --help )         usage ; shift ;;
+      # -- means the end of arguments
+      --)  shift; break ;;
+   esac
+done
+
+# From lukechilds gist discussion: https://gist.github.com/lukechilds/a83e1d7127b78fef38c2914c4ececc3c 
+# We use wget instead of curl here
+# Sample usage:
+#   VERSION_STRINGS=$(get_latest_release IntelRealSense/librealsense)
+
+function get_latest_release () {
+  # redirect wget to standard out and grep out the tag_name
+  wget -qO- https://api.github.com/repos/$1/releases/latest |
+    grep -Po '"tag_name": "\K.*?(?=")' 
+}
+
+if [[ $LIBREALSENSE_VERSION == "" ]] ; then
+   echo "Getting latest librealsense version number"
+   LIBREALSENSE_VERSION=$(get_latest_release IntelRealSense/librealsense)
+fi
+
+echo "Build with CUDA: "$USE_CUDA
+echo "Librealsense Version: $LIBREALSENSE_VERSION"
+
+red=`tput setaf 1`
+green=`tput setaf 2`
+reset=`tput sgr0`
+# e.g. echo "${red}The red tail hawk ${green}loves the green grass${reset}"
+
+if [ ! -d "$LIBREALSENSE_DIRECTORY" ] ; then
+  # clone librealsense
+  cd ${HOME}
+  echo "${green}Cloning librealsense${reset}"
+  git clone https://github.com/IntelRealSense/librealsense.git
+fi
+
+# Is the version of librealsense current enough?
+cd $LIBREALSENSE_DIRECTORY
+VERSION_TAG=$(git tag -l $LIBREALSENSE_VERSION)
+if [ ! $VERSION_TAG  ] ; then
+   echo ""
+  tput setaf 1
+  echo "==== librealsense Version Mismatch! ============="
+  tput sgr0
+  echo ""
+  echo "The installed version of librealsense is not current enough for these scripts."
+  echo "This script needs librealsense tag version: "$LIBREALSENSE_VERSION "but it is not available."
+  echo "Please upgrade librealsense or remove the librealsense folder before attempting to install again."
+  echo ""
+  exit 1
+fi
+
+# Checkout version the last tested version of librealsense
+git checkout $LIBREALSENSE_VERSION
+
+# Install the dependencies
+cd $INSTALL_DIR
+
+cd $LIBREALSENSE_DIRECTORY
+git checkout $LIBREALSENSE_VERSION
+
+# Now compile librealsense and install
+mkdir build 
+cd build
+# Build examples, including graphical ones
+echo "${green}Configuring Make system${reset}"
+# Build with CUDA (default), the CUDA flag is USE_CUDA, ie -DUSE_CUDA=true
+export CUDACXX=$NVCC_PATH
+export PATH=${PATH}:/usr/local/cuda/bin
+export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/cuda/lib64
+
+/usr/bin/cmake ../ -DBUILD_EXAMPLES=true -DFORCE_RSUSB_BACKEND=true -DBUILD_WITH_CUDA="$USE_CUDA" -DCMAKE_BUILD_TYPE=release -DBUILD_PYTHON_BINDINGS=bool:true
+
+# The library will be installed in /usr/local/lib, header files in /usr/local/include
+# The demos, tutorials and tests will located in /usr/local/bin.
+echo "${green}Building librealsense, headers, tools and demos${reset}"
+
+# If user didn't set # of jobs and we have > 4GB memory then
+# set # of jobs to # of cores-1, otherwise 1
+if [[ $NUM_PROCS == "" ]] ; then
+  TOTAL_MEMORY=$(free | awk '/Mem\:/ { print $2 }')
+  if [ $TOTAL_MEMORY -gt 4051048 ] ; then
+    NUM_CPU=$(nproc)
+    NUM_PROCS=$(($NUM_CPU - 1))
+  else
+    NUM_PROCS=1
+  fi
+fi
+
+time make -j$NUM_PROCS
+if [ $? -eq 0 ] ; then
+  echo "librealsense make successful"
+else
+  # Try to make again; Sometimes there are issues with the build
+  # because of lack of resources or concurrency issues
+  echo "librealsense did not build " >&2
+  echo "Retrying ... "
+  # Single thread this time
+  time make 
+  if [ $? -eq 0 ] ; then
+    echo "librealsense make successful"
+  else
+    # Try to make again
+    echo "librealsense did not successfully build" >&2
+    echo "Please fix issues and retry build"
+    exit 1
+  fi
+fi
+echo "${green}Installing librealsense, headers, tools and demos${reset}"
+sudo make install
+  
+if  grep -Fxq 'export PYTHONPATH=$PYTHONPATH:/usr/local/lib' ~/.bashrc ; then
+    echo "PYTHONPATH already exists in .bashrc file"
+else
+   echo 'export PYTHONPATH=$PYTHONPATH:/usr/local/lib' >> ~/.bashrc 
+   echo "PYTHONPATH added to ~/.bashrc. Pyhon wrapper is now available for importing pyrealsense2"
+fi
+
+cd $LIBREALSENSE_DIRECTORY
+echo "${green}Applying udev rules${reset}"
+# Copy over the udev rules so that camera can be run from user space
+sudo cp config/99-realsense-libusb.rules /etc/udev/rules.d/
+sudo udevadm control --reload-rules && udevadm trigger
+
+echo "${green}Library Installed${reset}"
+echo " "
+echo " -----------------------------------------"
+echo "The library is installed in /usr/local/lib"
+echo "The header files are in /usr/local/include"
+echo "The demos and tools are located in /usr/local/bin"
+echo " "
+echo " -----------------------------------------"
+echo " "
diff --git a/docker/scripts/hotplug-realsense.sh b/docker/scripts/hotplug-realsense.sh
new file mode 100755
index 0000000..7d515e5
--- /dev/null
+++ b/docker/scripts/hotplug-realsense.sh
@@ -0,0 +1,79 @@
+#!/usr/bin/env bash
+
+# Copyright (c) 2022, NVIDIA CORPORATION.  All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto.  Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+
+
+# This script is triggered when a corresponding udev rule is matched with action 
+function usage() {
+	echo "Usage: hotplug-realsense.sh -a <action=add/remove> -d <dev_path> [options]"
+	echo "-a | --action     		Action determines whether the device has been added or removed. Valid values are 'add' or 'remove'."
+	echo "-d | --dev-path   		Device path that will be used to mount the device."
+	echo "-M | --major-version      The kernel major number for the device. This will be used when action=add."
+	echo "-m | --minor-version      The kernel minor number for the device. This will be used when action=add."
+	echo "-p | --parent-path      	Parent node path of the device pointed by the -d flag."
+	echo "-h | --help       		Display this message."
+}
+
+function check_mandatory_param() {
+VAR=$1
+MSG=$2
+if [[ -z ${VAR} ]] ; then
+	echo  "${MSG}"
+	usage
+	exit 1
+fi
+}
+
+function timestamp() {
+	echo [$EPOCHREALTIME] [$(date)] 
+}
+
+ARGUMENTS=$(getopt -n hotplug-realsense.sh -o a:d:M:m:p:h -l action:,dev-path:,major-version:,minor-version:,parent-path:,help -- "$@" )
+
+if [[ $? -ne 0 ]]; then
+	usage
+fi
+
+eval set -- "$ARGUMENTS"
+
+while [ : ]; do
+  case "$1" in
+    -a | --action)
+        ACTION=$2 ; shift 2 ;;	
+    -d | --dev-path)
+        DEV_PATH=$2 ; shift 2 ;;
+    -M | --major-version)
+        MAJOR_VERSION=$2 ; shift 2 ;;
+    -m | --minor-version)
+        MINOR_VERSION=$2 ; shift 2 ;;
+	-p | --parent-path)
+        PARENT_PATH=/dev/$2 ; shift 2 ;;
+	-h | --help)
+        usage ; shift ;;
+    --) shift; break ;;
+  esac
+done
+
+check_mandatory_param ${ACTION:-""} "Please provide valid value for action" 
+check_mandatory_param ${DEV_PATH:-""} "Please provide valid value for device path" 
+
+if [[ "${ACTION}" == "add" ]]; then
+	check_mandatory_param ${MAJOR_VERSION:-""} "Please provide valid value for major number" 
+	check_mandatory_param ${MINOR_VERSION:-""} "Please provide valid value for minor number" 
+	sudo mknod -m a=rw ${DEV_PATH} c ${MAJOR_VERSION} ${MINOR_VERSION}
+	sudo chown root:plugdev ${DEV_PATH}
+	echo $(timestamp) "Added ${DEV_PATH} with major version: ${MAJOR_VERSION} and minor version: ${MINOR_VERSION} to docker" >> /tmp/docker_usb.log
+elif [[ "$ACTION" == "remove" ]]; then
+	sudo rm ${DEV_PATH} ${PARENT_PATH}
+	echo $(timestamp) "Removed ${DEV_PATH} ${PARENT_PATH} from docker" >> /tmp/docker_usb.log
+else
+	echo "Cannot recognize action=${ACTION}"
+	usage
+	exit 1
+fi
diff --git a/docker/scripts/install-3rdparty.sh b/docker/scripts/install-3rdparty.sh
new file mode 100644
index 0000000..d01902b
--- /dev/null
+++ b/docker/scripts/install-3rdparty.sh
@@ -0,0 +1,74 @@
+#!/bin/bash
+
+set -e
+
+# Install thirdparty libraries that CANNOT be installed using rosdep
+# Perfer to use rosdep to install thirdparty libraries if possible
+
+mkdir -p /workspace/thirdparty
+cd /workspace/thirdparty
+
+export CC=clang
+export CXX=clang++
+apt-get update
+
+# ============================================
+# Install magic_enum v0.9.5
+# For some reason, ros-humble-magic-enum doesn't work
+cd /workspace/thirdparty
+git clone https://github.com/Neargye/magic_enum.git
+cd magic_enum && git checkout v0.9.5
+mkdir build && cd build
+cmake \
+    -DCMAKE_BUILD_TYPE=Release \
+    -DMAGIC_ENUM_OPT_BUILD_TESTS=OFF \
+    -DMAGIC_ENUM_OPT_BUILD_EXAMPLES=OFF \
+    ..
+make -j$(nproc)
+make install
+cd /workspace/thirdparty && rm -rf magic_enum
+
+# ============================================
+# Install Open3D v.0.18.0
+# The libopen3d-dev package is too old (0.14.0).
+# cd /workspace/thirdparty
+# git clone https://github.com/isl-org/Open3D.git
+# cd Open3D && git checkout v0.18.0
+# apt-get install -y libc++-dev libc++abi-dev
+# mkdir build && cd build
+# cmake \
+#     -DCMAKE_BUILD_TYPE=Release \
+#     -DBUILD_SHARED_LIBS=ON \
+#     -DBUILD_EXAMPLES=OFF \
+#     -DBUILD_PYTHON_MODULE=OFF \
+#     ..
+# make -j$(nproc)
+# make install
+# cd /workspace/thirdparty && rm -rf Open3D
+
+
+# ============================================
+# Install google-glog v.0.7.0
+# Some packages depend on it and we can't use libgoogle-glog-dev due to
+# https://github.com/isl-org/Open3D/discussions/6515
+cd /workspace/thirdparty
+git clone https://github.com/google/glog.git
+cd glog && git checkout v0.7.0
+cmake -S . -B build -G "Unix Makefiles"
+cmake --build build
+cmake --build build --target install
+cd /workspace/thirdparty && rm -rf glog
+
+# ============================================
+# Install librealsense2
+apt-get install -y --no-install-recommends \
+    ros-humble-librealsense2* \
+    ros-humble-realsense2-*
+
+# ============================================
+# Install unitree_sdk2
+cd /workspace/thirdparty
+git clone https://github.com/unitreerobotics/unitree_sdk2.git
+cd unitree_sdk2
+./install.sh
+cd /workspace/thirdparty && rm -rf unitree_sdk2
\ No newline at end of file
diff --git a/docker/scripts/install-clang.sh b/docker/scripts/install-clang.sh
new file mode 100644
index 0000000..e9fb069
--- /dev/null
+++ b/docker/scripts/install-clang.sh
@@ -0,0 +1,23 @@
+#!/bin/bash
+
+set -e
+
+# Install LLVM family: compiler, formatter, debugger, etc
+
+apt-get update
+apt-get install -y --no-install-recommends \
+    clangd-15 \
+    clang-15 \
+    clang-format-15 \
+    lld-15 \
+    lldb-15
+update-alternatives --install /usr/bin/clang clang /usr/bin/clang-15 100
+update-alternatives --install /usr/bin/clangd clangd /usr/bin/clangd-15 100
+update-alternatives --install /usr/bin/clang++ clang++ /usr/bin/clang++-15 100
+update-alternatives --install /usr/bin/clang-format clang-format /usr/bin/clang-format-15 100
+update-alternatives --install /usr/bin/lld lld /usr/bin/lld-15 100
+update-alternatives --install /usr/bin/lldb lldb /usr/bin/lldb-15 100
+update-alternatives --install /usr/bin/lldb-server lldb-server /usr/bin/lldb-server-15 100
+
+# Fix https://github.com/llvm/llvm-project/issues/55575
+ln -s /usr/lib/llvm-15/lib/python3.10/dist-packages/lldb/* /usr/lib/python3/dist-packages/lldb/
\ No newline at end of file
diff --git a/docker/scripts/install-python-requirements.sh b/docker/scripts/install-python-requirements.sh
new file mode 100644
index 0000000..1fc9c58
--- /dev/null
+++ b/docker/scripts/install-python-requirements.sh
@@ -0,0 +1,12 @@
+#!/bin/bash
+
+set -e
+
+pip install \
+    "torch==2.2.1" \
+    "torchvision==0.17.1" \
+    "numpy>=1.26.4" \
+    "pybullet>=3.2.6" \
+    "proxsuite==0.6.3" \
+    "ipykernel==6.29.3" \
+    "open3d==0.18.0"
\ No newline at end of file
diff --git a/docker/scripts/install-realsense-dependencies.sh b/docker/scripts/install-realsense-dependencies.sh
new file mode 100644
index 0000000..db81a0c
--- /dev/null
+++ b/docker/scripts/install-realsense-dependencies.sh
@@ -0,0 +1,24 @@
+#!/bin/bash
+# install-realsense-dependencies.sh
+# Install dependencies for  the Intel Realsense library librealsense2 on a Jetson Nano Developer Kit
+# Copyright (c) 2016-19 Jetsonhacks 
+# MIT License
+
+red=`tput setaf 1`
+green=`tput setaf 2`
+reset=`tput sgr0`
+# e.g. echo "${red}red text ${green}green text${reset}"
+echo "${green}Adding Universe repository and updating${reset}"
+apt-add-repository universe
+apt-get update
+echo "${green}Adding dependencies, graphics libraries and tools${reset}"
+apt-get install libssl-dev libusb-1.0-0-dev pkg-config -y
+
+# Graphics libraries - for SDK's OpenGL-enabled examples
+apt-get install libgtk-3-dev libglfw3-dev libgl1-mesa-dev libglu1-mesa-dev -y
+
+# QtCreator for development; not required for librealsense core library
+apt-get install qtcreator -y
+
+# Add Python 3 support
+apt-get install -y python3 python3-dev
diff --git a/docker/scripts/install-ros2.sh b/docker/scripts/install-ros2.sh
new file mode 100644
index 0000000..2aa3f8a
--- /dev/null
+++ b/docker/scripts/install-ros2.sh
@@ -0,0 +1,32 @@
+#!/bin/bash
+
+set -e
+
+export DEBIAN_FRONTEND=noninteractive
+apt-get update && apt-get install -y locales
+locale-gen en_US en_US.UTF-8
+update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
+export LANG=en_US.UTF-8
+
+curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
+echo "deb [arch=amd64 signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu jammy main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null
+
+apt-get update && apt-get install -y --no-install-recommends \
+  ros-humble-desktop \
+  ros-dev-tools \
+  python3-colcon-common-extensions \
+  python3-flake8-docstrings \
+  python3-pip \
+  python3-pytest-cov \
+  ros-dev-tools \
+  python3-flake8-blind-except \
+  python3-flake8-builtins \
+  python3-flake8-class-newline \
+  python3-flake8-comprehensions \
+  python3-flake8-deprecated \
+  python3-flake8-import-order \
+  python3-flake8-quotes \
+  python3-pytest-repeat \
+  python3-pytest-rerunfailures \
+  ros-humble-rmw-cyclonedds-cpp \
+  ros-humble-rosidl-generator-dds-idl
\ No newline at end of file
diff --git a/docker/scripts/install-rosdep.sh b/docker/scripts/install-rosdep.sh
new file mode 100755
index 0000000..d9aa47e
--- /dev/null
+++ b/docker/scripts/install-rosdep.sh
@@ -0,0 +1,10 @@
+#!/bin/bash
+
+set -e
+
+# Make sure workspace is mounted in /tmp/ros_ws
+source /opt/ros/humble/setup.bash
+apt-get update
+rosdep init
+rosdep update
+rosdep install --from-paths /tmp/ros_ws --ignore-src -y
\ No newline at end of file
diff --git a/docker/scripts/install-tools.sh b/docker/scripts/install-tools.sh
new file mode 100644
index 0000000..7b4a7d3
--- /dev/null
+++ b/docker/scripts/install-tools.sh
@@ -0,0 +1,23 @@
+#!/bin/bash
+
+set -e
+
+# Install tools, utilities, and etc.
+
+apt-get update
+apt-get install -y --no-install-recommends \
+    curl \
+    git-lfs \
+    ninja-build \
+    psmisc \
+    python3-pip \
+    software-properties-common \
+    unzip \
+    usbutils \
+    vim \
+    wget \
+    net-tools \
+    iputils-ping
+
+pip3 install \
+    cmakelang==0.6.13
\ No newline at end of file
diff --git a/docker/scripts/install-unitree-ros2.sh b/docker/scripts/install-unitree-ros2.sh
new file mode 100644
index 0000000..ed9ac8c
--- /dev/null
+++ b/docker/scripts/install-unitree-ros2.sh
@@ -0,0 +1,16 @@
+#!/bin/bash
+
+set -e
+
+# clone unitree_ros2
+cd /workspace/go2-devcontainer/src
+git clone https://github.com/unitreerobotics/unitree_ros2
+
+# clone cyclonedds related packaged
+cd /workspace/go2-devcontainer/src/unitree_ros2/cyclonedds_ws/src
+git clone https://github.com/ros2/rmw_cyclonedds -b humble
+git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x 
+
+# build cyclonedds
+cd /workspace/go2-devcontainer/src/unitree_ros2/cyclonedds_ws
+colcon build --packages-select cyclonedds
\ No newline at end of file
diff --git a/docker/scripts/workspace-entrypoint.sh b/docker/scripts/workspace-entrypoint.sh
new file mode 100755
index 0000000..466cd88
--- /dev/null
+++ b/docker/scripts/workspace-entrypoint.sh
@@ -0,0 +1,21 @@
+#!/bin/bash
+#
+# Copyright (c) 2021, NVIDIA CORPORATION.  All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto.  Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+
+# Build ROS dependency
+echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc
+source /opt/ros/${ROS_DISTRO}/setup.bash
+
+sudo apt-get update
+rosdep update
+
+# Restart udev daemon
+sudo service udev restart
+
+$@
diff --git a/docker/tao/tao-converter-aarch64-tensorrt8.4.zip b/docker/tao/tao-converter-aarch64-tensorrt8.4.zip
new file mode 100644
index 0000000..94e8c6b
Binary files /dev/null and b/docker/tao/tao-converter-aarch64-tensorrt8.4.zip differ
diff --git a/docker/udev_rules/99-realsense-libusb-custom.rules b/docker/udev_rules/99-realsense-libusb-custom.rules
new file mode 100644
index 0000000..9279e0d
--- /dev/null
+++ b/docker/udev_rules/99-realsense-libusb-custom.rules
@@ -0,0 +1,11 @@
+##Version=0.1##
+# Device rules for Intel RealSense devices (D435 D435i D455)
+# For D435
+ACTION=="add" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b07", MODE:="0666", GROUP:="plugdev", RUN+="/opt/realsense/hotplug-realsense.sh -a add -d '%N' -M '%M' -m '%m'"
+ACTION=="remove" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b07", MODE:="0666", GROUP:="plugdev", RUN+="/opt/realsense/hotplug-realsense.sh -a remove -d '%N' -p '%P'"
+# For D435i
+ACTION=="add" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", MODE:="0666", GROUP:="plugdev", RUN+="/opt/realsense/hotplug-realsense.sh -a add -d '%N' -M '%M' -m '%m'"
+ACTION=="remove" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", MODE:="0666", GROUP:="plugdev", RUN+="/opt/realsense/hotplug-realsense.sh -a remove -d '%N' -p '%P'"
+# For D455
+ACTION=="add" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b5c", MODE:="0666", GROUP:="plugdev", RUN+="/opt/realsense/hotplug-realsense.sh -a add -d '%N' -M '%M' -m '%m'"
+ACTION=="remove" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b5c", MODE:="0666", GROUP:="plugdev", RUN+="/opt/realsense/hotplug-realsense.sh -a remove -d '%N' -p '%P'"