diff --git a/Dockerfile b/Dockerfile
index 6f109e4..e39e8bf 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -11,6 +11,12 @@ ARG USER_UID=1000
 ARG USER_GID=$USER_UID
 ENV ROS_DISTR=$ROS_DISTR
 
+RUN mkdir -p /ros2/src
+RUN mkdir -p /ros2_underlay/src
+
+ADD ros2/src /ros2/src
+ADD ros2_underlay/src /ros2_underlay/src
+
 RUN apt-get update \
     && apt-get -y install --no-install-recommends apt-utils dialog 2>&1 \
     && apt-get install -y \
@@ -28,12 +34,16 @@ RUN apt-get update \
     wget \
     byobu \
     xmlstarlet \
+    ros-${ROS_DISTR}-rqt-tf-tree \
+    ros-${ROS_DISTR}-apriltag \
     clang-format \
     && python3 -m pip install yapf \
     rope \
     flake8 \
     pylint \
     jedi \
+    && rosdep install --from-paths /ros2_underlay/src -y --ignore-src \
+#   && rosdep install --from paths /ros2/src -y --ignore-src \
     && groupadd --gid $USER_GID $USERNAME \
     && useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME \
     && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME\
@@ -59,14 +69,16 @@ RUN sh -c "$(wget -O- https://github.com/deluan/zsh-in-docker/releases/download/
     -a 'PATH=$PATH:$HOME/.local/bin' \
     -a 'export HISTFILE=/zsh_history/.zsh_history' \
     -a 'export TERM=xterm-256color' \
-    -a 'eval "$(register-python-argcomplete3 ros2)"' \
-    -a 'eval "$(register-python-argcomplete3 colcon)"' \
+    -a 'export COLORTERM=truecolor' \
     && mkdir -p "$HOME/.zsh" \
     && git clone https://github.com/sindresorhus/pure.git "$HOME/.zsh/pure" \
     && echo "source /opt/ros/$ROS_DISTR/setup.zsh" >> /home/$USERNAME/.zshrc
 
-ADD vimrc /home/$USERNAME/.vimrc
-ADD ycm_extra_conf.py /home/$USERNAME/.ycm_extra_conf.py
+RUN echo "source /opt/ros/$ROS_DISTR/setup.bash" >> /home/$USERNAME/.bashrc \
+    && echo "source $HOME/uuv/ros2_underlay/install/setup.bash" >> "$HOME/.bashrc"
+
+ADD ros2-vim/vimrc /home/$USERNAME/.vimrc
+ADD ros2-vim/ycm_extra_conf.py /home/$USERNAME/.ycm_extra_conf.py
 RUN git clone https://github.com/VundleVim/Vundle.vim.git /home/$USERNAME/.vim/bundle/Vundle.vim
 RUN vim +PluginInstall +qall
 RUN cd /home/$USERNAME/.vim/bundle/YouCompleteMe \
@@ -75,9 +87,9 @@ USER root
 RUN mkdir /zsh_history \
     && touch /zsh_history/.zsh_history \
     && chown -R $USERNAME /zsh_history
-ADD cpp.vim /home/$USERNAME/.vim/after/ftplugin/cpp.vim
+ADD ros2-vim/cpp.vim /home/$USERNAME/.vim/after/ftplugin/cpp.vim
 RUN chown -R $USERNAME /home/$USERNAME/
-ADD entrypoint.sh /entrypoint.sh
+ADD ros2-vim/entrypoint.sh /entrypoint.sh
 RUN chmod +x /entrypoint.sh
 RUN chown $USERNAME /entrypoint.sh
 USER ${USERNAME}
diff --git a/docker-compose.yml b/docker-compose.yml
index 46d651e..7a133ab 100644
--- a/docker-compose.yml
+++ b/docker-compose.yml
@@ -1,5 +1,6 @@
 services:
   nvidia:
+    container_name: ros2-vim
     extends:
       service: dev
     deploy:
@@ -10,10 +11,13 @@ services:
               count: all
               capabilities: [gpu]
   dev:
+    container_name: ros2-vim
     network_mode: host
     build:
-      context: .
-      dockerfile: ./Dockerfile
+      context: ..
+      dockerfile: ./ros2-vim/Dockerfile
+      ssh:
+        - default
     stdin_open: true
     tty: true
     entrypoint: ["/entrypoint.sh", "byobu"]
@@ -35,6 +39,7 @@ services:
         target: /var/run/avahi-daemon/socket
       - "ros2_zsh_history:/zsh_history"
       - $SSH_AUTH_SOCK:/ssh-agent
+      - "~/.gitconfig:/etc/gitconfig"
 
     environment:
       - DISPLAY=${DISPLAY}
diff --git a/entrypoint.sh b/entrypoint.sh
index 44723f4..a5033f6 100644
--- a/entrypoint.sh
+++ b/entrypoint.sh
@@ -5,6 +5,11 @@ WORKSPACE_DIR="\$HOME/uuv/ros2"
 echo "source $PX4_DIR/Tools/setup_gazebo.bash $PX4_DIR $PX4_DIR/build/px4_sitl_default > /dev/null" >> ~/.zshrc
 echo "export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:$PX4_DIR" >> ~/.zshrc
 echo "export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:$PX4_DIR/Tools/sitl_gazebo" >> ~/.zshrc
+
+echo 'source $HOME/uuv/ros2_underlay/install/setup.zsh' >> ~/.zshrc
+echo 'source $HOME/uuv/ros2/install/local_setup.zsh' >> ~/.zshrc
+echo 'eval "$(register-python-argcomplete3 ros2)"' >> ~/.zshrc
+echo 'eval "$(register-python-argcomplete3 colcon)"' >> ~/.zshrc
 vim +PluginUpdate +qall
 
 exec $@
diff --git a/vimrc b/vimrc
index c8de219..e3774a4 100644
--- a/vimrc
+++ b/vimrc
@@ -10,6 +10,7 @@ Plugin 'tmsvg/pear-tree'
 Plugin 'vim-airline/vim-airline'
 Plugin 'vim-airline/vim-airline-themes'
 Plugin 'aonemd/quietlight.vim'
+Plugin 'octol/vim-cpp-enhanced-highlight'
 
 call vundle#end()
 filetype plugin indent on
@@ -26,6 +27,8 @@ augroup WrapLine
 	autocmd FileType txt setlocal wrap
 	autocmd FileType rst setlocal wrap
 augroup End
+" highlight current line
+set cursorline
 " enable line numbers
 set number relativenumber
 set nu rnu
@@ -49,7 +52,7 @@ nnoremap <C-f> :NERDTreeFind<CR>
 
 set t_Co=256
 set background=light
-silent! colorscheme quietlight
+silent! colorscheme PaperColor
 let g:airline_theme='papercolor'
 :set colorcolumn=81,121