diff --git a/Dockerfile b/Dockerfile
index 7157c31..28368e9 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -48,10 +48,20 @@ RUN cd /tmp \
 RUN . /opt/ros/${ROS_DISTRO}/setup.sh \
  && bear catkin build
 
+RUN apt-get update \
+ && apt-get install -y software-properties-common \
+ && add-apt-repository ppa:deadsnakes/ppa \
+ && apt-get update \
+ && apt-get install -y python3.6 sudo
+RUN curl https://bootstrap.pypa.io/get-pip.py -o /tmp/get-pip.py \
+ && python3.6 /tmp/get-pip.py
+RUN pip3.6 install pyyaml rospkg catkin_pkg netifaces gcovr
+
 # build tests
 RUN . /opt/ros/${ROS_DISTRO}/setup.sh \
- && catkin build -DENABLE_TESTS=ON --continue-on-failure --no-status --make-args tests || exit 0
-#RUN catkin build -DENABLE_TESTS=ON --no-status orocos_kdl
+ && catkin config --cmake-args -DENABLE_TESTS=ON \
+ && (catkin build --continue-on-failure --no-status --make-args tests || exit 0) \
+ && catkin build --no-status
 
 # add entrypoint
 ENV ROS_WSPACE /ros_ws
@@ -65,15 +75,6 @@ exec \"\$@\"" > "${ROS_WSPACE}/entrypoint.sh" \
 ENTRYPOINT ["/ros_ws/entrypoint.sh"]
 CMD ["/bin/bash"]
 
-RUN apt-get update \
- && apt-get install -y software-properties-common \
- && add-apt-repository ppa:deadsnakes/ppa \
- && apt-get update \
- && apt-get install -y python3.6 sudo
-RUN curl https://bootstrap.pypa.io/get-pip.py -o /tmp/get-pip.py \
- && python3.6 /tmp/get-pip.py
-RUN pip3.6 install pyyaml rospkg catkin_pkg netifaces gcovr
-
 ENV TURTLEBOT_STAGE_MAP_FILE /ros_ws/src/turtlebot_simulator/turtlebot_stage/maps/maze.yaml
 ENV TURTLEBOT_STAGE_WORLD_FILE /ros_ws/src/turtlebot_simulator/turtlebot_stage/maps/stage/maze.world
 COPY _debug.launch .
diff --git a/Dockerfile.instrumentation b/Dockerfile.instrumentation
index 954c5c9..41deaf6 100644
--- a/Dockerfile.instrumentation
+++ b/Dockerfile.instrumentation
@@ -12,14 +12,15 @@ RUN cd /tmp \
 RUN . /opt/ros/${ROS_DISTRO}/setup.sh \
  && catkin clean -y \
  && catkin config --cmake-args \
+      -DENABLE_TESTS=ON \
       -DCMAKE_CXX_FLAGS="--coverage" \
       -DCMAKE_LD_FLAGS="--coverage" \
  && catkin build --no-status \
- && catkin build \
-      -DENABLE_TESTS=ON \
+ && (catkin build \
       --continue-on-failure \
       --no-status \
       --make-args tests \
-    || exit 0
+    || exit 0) \
+ && catkin build --no-status
 # clear any existing coverage information
 RUN gcovr -d -r .