diff --git a/.dockerignore b/.dockerignore
new file mode 100644
index 0000000..dca6942
--- /dev/null
+++ b/.dockerignore
@@ -0,0 +1,8 @@
+################################################################################
+# Repo
+
+.git/*
+.dockerignore
+.gitignore
+**Dockerfile
+**.Dockerfile
diff --git a/Dockerfile b/Dockerfile
new file mode 100644
index 0000000..16bdcc7
--- /dev/null
+++ b/Dockerfile
@@ -0,0 +1,86 @@
+ARG FROM_IMAGE=ros:eloquent
+
+# multi-stage for caching
+FROM $FROM_IMAGE AS cache
+
+# clone underlay source
+ENV UNDERLAY_WS /opt/underlay_ws
+RUN mkdir -p $UNDERLAY_WS/src
+WORKDIR $UNDERLAY_WS
+COPY ./tools/underlay.repos ./
+RUN vcs import src < underlay.repos
+
+# copy overlay source
+ENV OVERLAY_WS /opt/overlay_ws
+RUN mkdir -p $OVERLAY_WS/src
+WORKDIR $OVERLAY_WS
+COPY ./ src/navigation2
+# COPY ./tools/overlay.repos ./
+# RUN vcs import src < overlay.repos
+
+# copy manifests for caching
+WORKDIR /opt
+RUN find ./ -name "package.xml" | \
+      xargs cp --parents -t /tmp
+    # && find ./ -name "COLCON_IGNORE" | \
+    #   xargs cp --parents -t /tmp
+
+# multi-stage for building
+FROM $FROM_IMAGE AS build
+
+# install CI dependencies
+RUN apt-get update && apt-get install -q -y \
+      ccache \
+      lcov \
+    && rm -rf /var/lib/apt/lists/*
+
+# copy underlay manifests
+ENV UNDERLAY_WS /opt/underlay_ws
+COPY --from=cache /tmp/underlay_ws $UNDERLAY_WS
+WORKDIR $UNDERLAY_WS
+
+# install underlay dependencies
+RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
+    apt-get update && rosdep install -q -y \
+      --from-paths src \
+      --ignore-src \
+    && rm -rf /var/lib/apt/lists/*
+
+# copy underlay source
+COPY --from=cache $UNDERLAY_WS ./
+
+# build underlay source
+ARG UNDERLAY_MIXINS="release ccache"
+RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
+    colcon build \
+      --symlink-install \
+      --mixin $UNDERLAY_MIXINS
+
+# copy overlay manifests
+ENV OVERLAY_WS /opt/overlay_ws
+COPY --from=cache /tmp/overlay_ws $OVERLAY_WS
+WORKDIR $OVERLAY_WS
+
+# install overlay dependencies
+RUN . $UNDERLAY_WS/install/setup.sh && \
+    apt-get update && rosdep install -q -y \
+      --from-paths \
+        src \
+        $UNDERLAY_WS/src \
+      --ignore-src \
+    && rm -rf /var/lib/apt/lists/*
+
+# copy overlay source
+COPY --from=cache $OVERLAY_WS ./
+
+# build overlay source
+ARG OVERLAY_MIXINS="release ccache"
+RUN . $UNDERLAY_WS/install/setup.sh && \
+    colcon build \
+      --symlink-install \
+      --mixin $OVERLAY_MIXINS
+
+# source overlay from entrypoint
+RUN sed --in-place \
+      's|^source .*|source "$OVERLAY_WS/install/setup.bash"|' \
+      /ros_entrypoint.sh
\ No newline at end of file
diff --git a/realsense_examples/package.xml b/realsense_examples/package.xml
index b9e1ceb..d99f051 100644
--- a/realsense_examples/package.xml
+++ b/realsense_examples/package.xml
@@ -14,7 +14,7 @@
 
   <depend>realsense_ros</depend>
   <depend>realsense_node</depend>
-  <depend>realsense2</depend>
+  <depend>librealsense2</depend>
   <depend>realsense_msgs</depend>
 
   <exec_depend>launch_ros</exec_depend>
diff --git a/realsense_node/CMakeLists.txt b/realsense_node/CMakeLists.txt
index 3935952..a242862 100644
--- a/realsense_node/CMakeLists.txt
+++ b/realsense_node/CMakeLists.txt
@@ -73,6 +73,11 @@ find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(realsense_ros REQUIRED)
 find_package(realsense_msgs REQUIRED)
+find_package(realsense2 REQUIRED)
+
+include_directories(
+  ${realsense2_INCLUDE_DIR}
+)
 
 add_executable(realsense_node
   src/realsense_node.cpp
diff --git a/realsense_ros/CMakeLists.txt b/realsense_ros/CMakeLists.txt
index 1b2b885..d47b6ab 100644
--- a/realsense_ros/CMakeLists.txt
+++ b/realsense_ros/CMakeLists.txt
@@ -65,6 +65,7 @@ set(CMAKE_CXX_FLAGS "-fPIE -fPIC -D_FORTIFY_SOURCE=2 -fstack-protector -Wformat
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(rcl REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(nav_msgs REQUIRED)
@@ -82,7 +83,7 @@ endif()
 
 include_directories(
   ${CMAKE_CURRENT_SOURCE_DIR}/include
-  ${realsense2_INCLUDE_DIRS}
+  ${realsense2_INCLUDE_DIR}
 )
 
 set(node_plugins "")
@@ -96,10 +97,11 @@ add_library(${PROJECT_NAME} SHARED
 )
 
 target_link_libraries(${PROJECT_NAME}
-  realsense2
+  ${realsense2_LIBRARY}
 )
 
 ament_target_dependencies(${PROJECT_NAME}
+  rcl
   rclcpp
   rclcpp_components
   nav_msgs
@@ -133,6 +135,7 @@ install(
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
+  find_package(rcl REQUIRED)
   find_package(rclcpp REQUIRED)
   find_package(nav_msgs REQUIRED)
   find_package(sensor_msgs REQUIRED)
@@ -159,9 +162,10 @@ if(BUILD_TESTING)
         ${${PROJECT_NAME}_INCLUDE_DIRS}
       )
       target_link_libraries(${target}
-        realsense2
+        ${realsense2_LIBRARY}
 	${PROJECT_NAME})
       ament_target_dependencies(${target}
+        "rcl"
         "rclcpp"
         "nav_msgs"
         "sensor_msgs"
diff --git a/realsense_ros/include/realsense/rs_base.hpp b/realsense_ros/include/realsense/rs_base.hpp
index ecd5381..1d05b12 100644
--- a/realsense_ros/include/realsense/rs_base.hpp
+++ b/realsense_ros/include/realsense/rs_base.hpp
@@ -80,6 +80,7 @@ class RealSenseBase
   Result toggleStream(const stream_index_pair & stream, const rclcpp::Parameter & param);
   Result changeResolution(const stream_index_pair & stream, const rclcpp::Parameter & param);
   Result changeFPS(const stream_index_pair & stream, const rclcpp::Parameter & param);
+  rclcpp::Time frameToTime(const rs2::frame & frame);
 
   typedef struct VideoStreamInfo
   {
diff --git a/realsense_ros/package.xml b/realsense_ros/package.xml
index a85f0b7..d44a3b8 100644
--- a/realsense_ros/package.xml
+++ b/realsense_ros/package.xml
@@ -17,10 +17,12 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>pcl_conversions</depend>
-  <depend>realsense2</depend>
+  <depend>librealsense2</depend>
   <depend>realsense_msgs</depend>
   <depend>image_transport</depend>
 
+  <build_depend>rcl</build_depend>
+
   <exec_depend>launch_ros</exec_depend>
   
   <test_depend>ament_lint_auto</test_depend>
diff --git a/realsense_ros/src/rs_base.cpp b/realsense_ros/src/rs_base.cpp
index 355f978..b47bf90 100644
--- a/realsense_ros/src/rs_base.cpp
+++ b/realsense_ros/src/rs_base.cpp
@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <sstream>
+#include "rcl/time.h"
 #include "realsense/rs_base.hpp"
 
 namespace realsense
@@ -457,4 +458,8 @@ Result RealSenseBase::changeFPS(const stream_index_pair & stream, const rclcpp::
   }
   return result;
 }
+
+rclcpp::Time RealSenseBase::frameToTime(const rs2::frame & frame) {
+  return rclcpp::Time(RCL_MS_TO_NS(frame.get_timestamp()));
+}
 }  // namespace realsense
diff --git a/realsense_ros/src/rs_d435.cpp b/realsense_ros/src/rs_d435.cpp
index bcd8786..0d2a202 100644
--- a/realsense_ros/src/rs_d435.cpp
+++ b/realsense_ros/src/rs_d435.cpp
@@ -60,7 +60,7 @@ RealSenseD435::RealSenseD435(rs2::context ctx, rs2::device dev, rclcpp::Node & n
 void RealSenseD435::publishTopicsCallback(const rs2::frame & frame)
 {
   rs2::frameset frameset = frame.as<rs2::frameset>();
-  rclcpp::Time t = node_.now();
+  rclcpp::Time t = frameToTime(frame);
   if (enable_[COLOR] && (image_pub_[COLOR]->get_subscription_count() > 0 || camera_info_pub_[COLOR]->get_subscription_count() > 0)){
     auto frame = frameset.get_color_frame();
     publishImageTopic(frame, t);
diff --git a/realsense_ros/src/rs_d435i.cpp b/realsense_ros/src/rs_d435i.cpp
index b452de2..a2b1662 100644
--- a/realsense_ros/src/rs_d435i.cpp
+++ b/realsense_ros/src/rs_d435i.cpp
@@ -35,7 +35,7 @@ RealSenseD435I::RealSenseD435I(rs2::context ctx, rs2::device dev, rclcpp::Node &
 
 void RealSenseD435I::publishTopicsCallback(const rs2::frame & frame)
 {
-  rclcpp::Time t = node_.now();
+  rclcpp::Time t = frameToTime(frame);
   if (frame.is<rs2::frameset>()) {
     RealSenseD435::publishTopicsCallback(frame);
   } else if (frame.is<rs2::motion_frame>()) {
diff --git a/realsense_ros/src/rs_t265.cpp b/realsense_ros/src/rs_t265.cpp
index a44b30f..bde3b1a 100644
--- a/realsense_ros/src/rs_t265.cpp
+++ b/realsense_ros/src/rs_t265.cpp
@@ -42,7 +42,7 @@ RealSenseT265::RealSenseT265(rs2::context ctx, rs2::device dev, rclcpp::Node & n
 
 void RealSenseT265::publishTopicsCallback(const rs2::frame & frame)
 {
-  rclcpp::Time t = node_.now();
+  rclcpp::Time t = frameToTime(frame);
 
   if (frame.is<rs2::frameset>()) {
     auto frameset = frame.as<rs2::frameset>();
diff --git a/tools/overlay.repos b/tools/overlay.repos
new file mode 100644
index 0000000..374b2a6
--- /dev/null
+++ b/tools/overlay.repos
@@ -0,0 +1,5 @@
+repositories:
+  intel/ros2_intel_realsense:
+    type: git
+    url: https://github.com/ruffsl/ros2_intel_realsense.git
+    version: docker
diff --git a/tools/underlay.repos b/tools/underlay.repos
new file mode 100644
index 0000000..1c1baaa
--- /dev/null
+++ b/tools/underlay.repos
@@ -0,0 +1,5 @@
+repositories:
+  intel/librealsense:
+    type: git
+    url: https://github.com/ruffsl/librealsense.git
+    version: docker