diff --git a/.dockerignore b/.dockerignore
new file mode 100644
index 0000000..6647fc9
--- /dev/null
+++ b/.dockerignore
@@ -0,0 +1,4 @@
+**/Dockerfile*
+**/.dockerignore
+**/.git
+**/.travis.yml
diff --git a/.travis.yml b/.travis.yml
index 7a41d46..73b061d 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -20,12 +20,15 @@ services: docker
 cache:
   directories:
     - /home/travis/docker/
+
 env:
-  global:
-    - DOCKER_CACHE_FILE=/home/travis/docker/cache.tar.gz
+  matrix:
+    - ROS_RELEASE=indigo DOCKER_CACHE_FILE=/home/travis/docker/indigo-cache.tar.gz
+    - ROS_RELEASE=kinetic DOCKER_CACHE_FILE=/home/travis/docker/kinetic-cache.tar.gz
+
 before_install: scripts/load_docker_cache.sh
 
 install: true
 script:
-  - docker build ${TRAVIS_BUILD_DIR} -t cartographer_ros
+  - docker build ${TRAVIS_BUILD_DIR} -t cartographer_ros:${ROS_RELEASE} -f Dockerfile.${ROS_RELEASE}
   - scripts/save_docker_cache.sh
diff --git a/Dockerfile b/Dockerfile.indigo
similarity index 96%
rename from Dockerfile
rename to Dockerfile.indigo
index e9d5160..ec5f6c5 100644
--- a/Dockerfile
+++ b/Dockerfile.indigo
@@ -35,7 +35,9 @@ COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_r
 COPY scripts/install_debs.sh cartographer_ros/scripts/
 RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/*
 
-# Build, install, and test all packages individually to allow caching.
+# Build, install, and test all packages individually to allow caching. The
+# ordering of these steps must match the topological package ordering as
+# determined by Catkin.
 COPY scripts/install.sh cartographer_ros/scripts/
 COPY scripts/catkin_test_results.sh cartographer_ros/scripts/
 
diff --git a/Dockerfile.kinetic b/Dockerfile.kinetic
new file mode 100644
index 0000000..48ad092
--- /dev/null
+++ b/Dockerfile.kinetic
@@ -0,0 +1,73 @@
+# Copyright 2016 The Cartographer Authors
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+FROM ros:kinetic
+
+# Xenial's base image doesn't ship with sudo.
+RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/*
+
+# First, we invalidate the entire cache if googlecartographer/cartographer has
+# changed. This file's content changes whenever master changes. See:
+# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone
+ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master \
+    cartographer_ros/cartographer_version.json
+
+# wstool needs the updated rosinstall file to clone the correct repos.
+COPY cartographer_ros.rosinstall cartographer_ros/
+COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/
+# Remove the contents of the cartographer_ros repo and copy in the updated
+# files as necessary.
+RUN cartographer_ros/scripts/prepare_catkin_workspace.sh && \
+    rm -rf catkin_ws/src/cartographer_ros/*
+
+# rosdep needs the updated package.xml files to install the correct debs.
+COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/
+COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
+COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/
+COPY scripts/install_debs.sh cartographer_ros/scripts/
+RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/*
+
+# Build, install, and test all packages individually to allow caching. The
+# ordering of these steps must match the topological package ordering as
+# determined by Catkin.
+COPY scripts/install.sh cartographer_ros/scripts/
+COPY scripts/catkin_test_results.sh cartographer_ros/scripts/
+
+RUN cartographer_ros/scripts/install.sh --pkg ceres-solver
+
+RUN cartographer_ros/scripts/install.sh --pkg cartographer && \
+    cartographer_ros/scripts/install.sh --pkg cartographer --make-args test
+
+COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
+RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \
+    cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \
+        --catkin-make-args run_tests && \
+    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs
+
+COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/
+RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \
+    cartographer_ros/scripts/install.sh --pkg cartographer_ros \
+        --catkin-make-args run_tests && \
+    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros
+
+COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/
+RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \
+    cartographer_ros/scripts/install.sh --pkg cartographer_rviz \
+        --catkin-make-args run_tests && \
+    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz
+
+COPY scripts/ros_entrypoint.sh /
+# A BTRFS bug may prevent us from cleaning up these directories.
+# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory
+RUN rm -rf cartographer_ros catkin_ws || true
diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt
index f56b1cc..e4b3e5f 100644
--- a/cartographer_ros/CMakeLists.txt
+++ b/cartographer_ros/CMakeLists.txt
@@ -20,12 +20,14 @@ set(PACKAGE_DEPENDENCIES
   cartographer_ros_msgs
   eigen_conversions
   geometry_msgs
+  message_runtime
   nav_msgs
   pcl_conversions
   rosbag
   roscpp
   roslib
   sensor_msgs
+  std_msgs
   tf2
   tf2_eigen
   tf2_ros
@@ -41,7 +43,7 @@ google_enable_testing()
 find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})
 
 include(FindPkgConfig)
-PKG_SEARCH_MODULE(YAMLCPP REQUIRED yaml-cpp>=0.5.1)
+pkg_search_module(YAMLCPP REQUIRED yaml-cpp>=0.5.1)
 
 find_package(PCL REQUIRED COMPONENTS common io)
 find_package(Eigen3 REQUIRED)
@@ -54,14 +56,20 @@ if(DEFINED urdfdom_headers_VERSION)
   endif()
 endif()
 
+include_directories(
+  ${urdfdom_headers_INCLUDE_DIRS}
+)
+
 catkin_package(
   CATKIN_DEPENDS
-    message_runtime
     ${PACKAGE_DEPENDENCIES}
   DEPENDS
+    CARTOGRAPHER
+    YAMLCPP
     PCL
-    Eigen3
+    EIGEN3
     Boost
+    urdfdom_headers
   INCLUDE_DIRS "."
   LIBRARIES ${PROJECT_NAME}
 )
@@ -83,7 +91,7 @@ install(PROGRAMS scripts/tf_remove_frames.py
 )
 
 install(TARGETS ${PROJECT_NAME}
-        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-        RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
 )
diff --git a/cartographer_ros/package.xml b/cartographer_ros/package.xml
index b8cbec7..1837a42 100644
--- a/cartographer_ros/package.xml
+++ b/cartographer_ros/package.xml
@@ -39,7 +39,6 @@
 
   <depend>cartographer</depend>
   <depend>cartographer_ros_msgs</depend>
-
   <depend>eigen_conversions</depend>
   <depend>geometry_msgs</depend>
   <depend>libgflags-dev</depend>
diff --git a/cartographer_ros_msgs/CMakeLists.txt b/cartographer_ros_msgs/CMakeLists.txt
index 18f1c56..2e02b61 100644
--- a/cartographer_ros_msgs/CMakeLists.txt
+++ b/cartographer_ros_msgs/CMakeLists.txt
@@ -16,11 +16,15 @@ cmake_minimum_required(VERSION 2.8.3)
 
 project(cartographer_ros_msgs)
 
-find_package(Cartographer REQUIRED)
+set(PACKAGE_DEPENDENCIES
+  geometry_msgs
+)
+
+find_package(cartographer REQUIRED)
 include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake")
 google_initialize_cartographer_project()
 
-find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation)
+find_package(catkin REQUIRED COMPONENTS message_generation ${PACKAGE_DEPENDENCIES})
 
 add_message_files(
   FILES
@@ -40,4 +44,7 @@ generate_messages(
     geometry_msgs
 )
 
-catkin_package()
+catkin_package(
+  CATKIN_DEPENDS
+    ${PACKAGE_DEPENDENCIES}
+)
diff --git a/cartographer_ros_msgs/package.xml b/cartographer_ros_msgs/package.xml
index fae9f52..421282a 100644
--- a/cartographer_ros_msgs/package.xml
+++ b/cartographer_ros_msgs/package.xml
@@ -31,6 +31,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>cartographer</depend>
+  <depend>geometry_msgs</depend>
 
   <build_depend>message_generation</build_depend>
 </package>
diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt
index 060550e..5fd90b6 100644
--- a/cartographer_rviz/CMakeLists.txt
+++ b/cartographer_rviz/CMakeLists.txt
@@ -19,11 +19,13 @@ project(cartographer_rviz)
 set(PACKAGE_DEPENDENCIES
   cartographer_ros_msgs
   eigen_conversions
+  message_runtime
   roscpp
+  roslib
   rviz
 )
 
-find_package(Cartographer REQUIRED)
+find_package(cartographer REQUIRED)
 include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake")
 google_initialize_cartographer_project()
 
diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml
index cd24a14..21e12ad 100644
--- a/cartographer_rviz/package.xml
+++ b/cartographer_rviz/package.xml
@@ -36,7 +36,6 @@
 
   <depend>cartographer</depend>
   <depend>cartographer_ros_msgs</depend>
-
   <depend>eigen_conversions</depend>
   <depend>libqt5-core</depend>
   <depend>libqt5-gui</depend>
diff --git a/scripts/save_docker_cache.sh b/scripts/save_docker_cache.sh
index d22e1ed..03d0976 100755
--- a/scripts/save_docker_cache.sh
+++ b/scripts/save_docker_cache.sh
@@ -24,6 +24,6 @@ set -o pipefail
 if [[ ${TRAVIS_BRANCH} == "master" ]] &&
     [[ ${TRAVIS_PULL_REQUEST} == "false" ]]; then
   mkdir -p $(dirname ${DOCKER_CACHE_FILE});
-  docker save $(docker history -q cartographer_ros |
+  docker save $(docker history -q cartographer_ros:${ROS_RELEASE} |
       grep -v '<missing>') | gzip > ${DOCKER_CACHE_FILE};
 fi