diff --git a/openni2_camera/CMakeLists.txt b/openni2_camera/CMakeLists.txt
index e7b7b71..65bbcd5 100644
--- a/openni2_camera/CMakeLists.txt
+++ b/openni2_camera/CMakeLists.txt
@@ -5,7 +5,6 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
 endif()
 
-find_package(Boost REQUIRED COMPONENTS system thread)
 find_package(ament_cmake REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(camera_info_manager REQUIRED)
@@ -28,7 +27,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
 
 include_directories(
   include
-  ${Boost_INCLUDE_DIRS}
   ${PC_OPENNI2_INCLUDE_DIRS}
 )
 
@@ -45,7 +43,6 @@ add_library(openni2_wrapper SHARED
   src/openni2_video_mode.cpp
 )
 target_link_libraries(openni2_wrapper
-  ${Boost_LIBRARIES}
   ${PC_OPENNI2_LIBRARIES}
 )
 ament_target_dependencies(openni2_wrapper
@@ -59,14 +56,13 @@ ament_target_dependencies(openni2_wrapper
 )
 
 #add_executable(test_wrapper test/test_wrapper.cpp)
-#target_link_libraries(test_wrapper openni2_wrapper ${Boost_LIBRARIES})
+#target_link_libraries(test_wrapper openni2_wrapper)
 
 add_library(openni2_camera_lib SHARED
   src/openni2_driver.cpp
 )
 target_link_libraries(openni2_camera_lib
   openni2_wrapper
-  ${Boost_LIBRARIE}
 )
 ament_target_dependencies(openni2_camera_lib
   camera_info_manager
diff --git a/openni2_camera/include/openni2_camera/openni2_device.h b/openni2_camera/include/openni2_camera/openni2_device.h
index 7a9f5a2..072cdd3 100644
--- a/openni2_camera/include/openni2_camera/openni2_device.h
+++ b/openni2_camera/include/openni2_camera/openni2_device.h
@@ -36,10 +36,7 @@
 
 #include "openni2_camera/openni2_exception.h"
 
-#include <boost/shared_ptr.hpp>
-#include <boost/cstdint.hpp>
-#include <boost/bind.hpp>
-#include <boost/function.hpp>
+#include <functional>
 
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/image.hpp>
@@ -58,7 +55,7 @@ class SensorInfo;
 namespace openni2_wrapper
 {
 
-typedef boost::function<void(sensor_msgs::msg::Image::SharedPtr image)> FrameCallbackFunction;
+typedef std::function<void(sensor_msgs::msg::Image::SharedPtr image)> FrameCallbackFunction;
 
 class OpenNI2FrameListener;
 
@@ -139,20 +136,20 @@ class OpenNI2Device
 protected:
   void shutdown();
 
-  boost::shared_ptr<openni::VideoStream> getIRVideoStream() const;
-  boost::shared_ptr<openni::VideoStream> getColorVideoStream() const;
-  boost::shared_ptr<openni::VideoStream> getDepthVideoStream() const;
+  std::shared_ptr<openni::VideoStream> getIRVideoStream() const;
+  std::shared_ptr<openni::VideoStream> getColorVideoStream() const;
+  std::shared_ptr<openni::VideoStream> getDepthVideoStream() const;
 
-  boost::shared_ptr<openni::Device> openni_device_;
-  boost::shared_ptr<openni::DeviceInfo> device_info_;
+  std::shared_ptr<openni::Device> openni_device_;
+  std::shared_ptr<openni::DeviceInfo> device_info_;
 
-  boost::shared_ptr<OpenNI2FrameListener> ir_frame_listener;
-  boost::shared_ptr<OpenNI2FrameListener> color_frame_listener;
-  boost::shared_ptr<OpenNI2FrameListener> depth_frame_listener;
+  std::shared_ptr<OpenNI2FrameListener> ir_frame_listener;
+  std::shared_ptr<OpenNI2FrameListener> color_frame_listener;
+  std::shared_ptr<OpenNI2FrameListener> depth_frame_listener;
 
-  mutable boost::shared_ptr<openni::VideoStream> ir_video_stream_;
-  mutable boost::shared_ptr<openni::VideoStream> color_video_stream_;
-  mutable boost::shared_ptr<openni::VideoStream> depth_video_stream_;
+  mutable std::shared_ptr<openni::VideoStream> ir_video_stream_;
+  mutable std::shared_ptr<openni::VideoStream> color_video_stream_;
+  mutable std::shared_ptr<openni::VideoStream> depth_video_stream_;
 
   mutable std::vector<OpenNI2VideoMode> ir_video_modes_;
   mutable std::vector<OpenNI2VideoMode> color_video_modes_;
diff --git a/openni2_camera/include/openni2_camera/openni2_device_info.h b/openni2_camera/include/openni2_camera/openni2_device_info.h
index 0cb7805..fef11b7 100644
--- a/openni2_camera/include/openni2_camera/openni2_device_info.h
+++ b/openni2_camera/include/openni2_camera/openni2_device_info.h
@@ -34,8 +34,6 @@
 
 #include <ostream>
 
-#include <boost/cstdint.hpp>
-
 namespace openni2_wrapper
 {
 
diff --git a/openni2_camera/include/openni2_camera/openni2_device_manager.h b/openni2_camera/include/openni2_camera/openni2_device_manager.h
index 03551cf..10a73c4 100644
--- a/openni2_camera/include/openni2_camera/openni2_device_manager.h
+++ b/openni2_camera/include/openni2_camera/openni2_device_manager.h
@@ -34,8 +34,6 @@
 
 #include "openni2_camera/openni2_device_info.h"
 
-#include <boost/thread/mutex.hpp>
-
 #include <rclcpp/rclcpp.hpp>
 
 #include <vector>
@@ -54,21 +52,21 @@ class OpenNI2DeviceManager
   OpenNI2DeviceManager();
   virtual ~OpenNI2DeviceManager();
 
-  static boost::shared_ptr<OpenNI2DeviceManager> getSingelton();
+  static std::shared_ptr<OpenNI2DeviceManager> getSingelton();
 
-  boost::shared_ptr<std::vector<OpenNI2DeviceInfo> > getConnectedDeviceInfos() const;
-  boost::shared_ptr<std::vector<std::string> > getConnectedDeviceURIs() const;
+  std::shared_ptr<std::vector<OpenNI2DeviceInfo> > getConnectedDeviceInfos() const;
+  std::shared_ptr<std::vector<std::string> > getConnectedDeviceURIs() const;
   std::size_t getNumOfConnectedDevices() const;
 
-  boost::shared_ptr<OpenNI2Device> getAnyDevice(rclcpp::Node* node);
-  boost::shared_ptr<OpenNI2Device> getDevice(const std::string& device_URI, rclcpp::Node* node);
+  std::shared_ptr<OpenNI2Device> getAnyDevice(rclcpp::Node* node);
+  std::shared_ptr<OpenNI2Device> getDevice(const std::string& device_URI, rclcpp::Node* node);
 
   std::string getSerial(const std::string& device_URI) const;
 
 protected:
-  boost::shared_ptr<OpenNI2DeviceListener> device_listener_;
+  std::shared_ptr<OpenNI2DeviceListener> device_listener_;
 
-  static boost::shared_ptr<OpenNI2DeviceManager> singelton_;
+  static std::shared_ptr<OpenNI2DeviceManager> singelton_;
 };
 
 
diff --git a/openni2_camera/include/openni2_camera/openni2_driver.h b/openni2_camera/include/openni2_camera/openni2_driver.h
index 33a1bd5..7be443d 100644
--- a/openni2_camera/include/openni2_camera/openni2_driver.h
+++ b/openni2_camera/include/openni2_camera/openni2_driver.h
@@ -32,10 +32,7 @@
 #ifndef OPENNI2_DRIVER_H
 #define OPENNI2_DRIVER_H
 
-#include <boost/shared_ptr.hpp>
-#include <boost/cstdint.hpp>
-#include <boost/bind.hpp>
-#include <boost/function.hpp>
+#include <functional>
 
 #include <sensor_msgs/msg/image.hpp>
 
@@ -110,8 +107,8 @@ class OpenNI2Driver : public rclcpp::Node
 
   void forceSetExposure();
 
-  boost::shared_ptr<OpenNI2DeviceManager> device_manager_;
-  boost::shared_ptr<OpenNI2Device> device_;
+  std::shared_ptr<OpenNI2DeviceManager> device_manager_;
+  std::shared_ptr<OpenNI2Device> device_;
 
   std::string device_id_;
   int bus_id_;
@@ -131,7 +128,7 @@ class OpenNI2Driver : public rclcpp::Node
   /** \brief get_serial server*/
   rclcpp::Service<openni2_camera::srv::GetSerial>::SharedPtr get_serial_server;
 
-  boost::mutex connect_mutex_;
+  std::mutex connect_mutex_;
   // published topics
   image_transport::CameraPublisher pub_color_;
   image_transport::CameraPublisher pub_depth_;
@@ -145,6 +142,8 @@ class OpenNI2Driver : public rclcpp::Node
   /** \brief Camera info manager objects. */
   std::shared_ptr<camera_info_manager::CameraInfoManager> color_info_manager_, ir_info_manager_;
 
+  OnSetParametersCallbackHandle::SharedPtr parameters_callback_;
+
   OpenNI2VideoMode ir_video_mode_;
   OpenNI2VideoMode color_video_mode_;
   OpenNI2VideoMode depth_video_mode_;
diff --git a/openni2_camera/include/openni2_camera/openni2_frame_listener.h b/openni2_camera/include/openni2_camera/openni2_frame_listener.h
index ee871d5..c5b21fc 100644
--- a/openni2_camera/include/openni2_camera/openni2_frame_listener.h
+++ b/openni2_camera/include/openni2_camera/openni2_frame_listener.h
@@ -69,7 +69,7 @@ class OpenNI2FrameListener : public openni::VideoStream::NewFrameListener
   FrameCallbackFunction callback_;
 
   bool user_device_timer_;
-  boost::shared_ptr<OpenNI2TimerFilter> timer_filter_;
+  std::shared_ptr<OpenNI2TimerFilter> timer_filter_;
 
   rclcpp::Node* node_;  // needed for time
   rclcpp::Time prev_time_stamp_;
diff --git a/openni2_camera/launch/camera_only.launch.py b/openni2_camera/launch/camera_only.launch.py
index 7a82146..a6fdb64 100644
--- a/openni2_camera/launch/camera_only.launch.py
+++ b/openni2_camera/launch/camera_only.launch.py
@@ -32,8 +32,6 @@
 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-import os
-
 import launch
 import launch_ros.actions
 import launch_ros.descriptions
diff --git a/openni2_camera/launch/camera_with_cloud.launch.py b/openni2_camera/launch/camera_with_cloud.launch.py
index f015ac2..d71370e 100644
--- a/openni2_camera/launch/camera_with_cloud.launch.py
+++ b/openni2_camera/launch/camera_with_cloud.launch.py
@@ -32,8 +32,6 @@
 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-import os
-
 import launch
 import launch_ros.actions
 import launch_ros.descriptions
diff --git a/openni2_camera/openni2_nodelets.xml b/openni2_camera/openni2_nodelets.xml
deleted file mode 100644
index 499ebaf..0000000
--- a/openni2_camera/openni2_nodelets.xml
+++ /dev/null
@@ -1,10 +0,0 @@
-<!-- OpenNI2 driver nodelet publisher -->
-<library path="lib/libopenni2_camera_nodelet">
-
-  <class name="openni2_camera/OpenNI2DriverNodelet" type="openni2_camera::OpenNI2DriverNodelet" base_class_type="nodelet::Nodelet">
-    <description>
-      OpenNI2 camera driver nodelet.
-    </description>
-  </class>
-
-</library>
diff --git a/openni2_camera/package.xml b/openni2_camera/package.xml
index 99824e2..f55b69e 100644
--- a/openni2_camera/package.xml
+++ b/openni2_camera/package.xml
@@ -14,7 +14,6 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
-  <depend>boost</depend>
   <depend>builtin_interfaces</depend>
   <depend>camera_info_manager</depend>
   <depend>libopenni2-dev</depend>
diff --git a/openni2_camera/ros/openni2_camera_node.cpp b/openni2_camera/ros/openni2_camera_node.cpp
deleted file mode 100644
index 6fb4133..0000000
--- a/openni2_camera/ros/openni2_camera_node.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- * Copyright (c) 2013, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyright
- *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the Willow Garage, Inc. nor the names of its
- *       contributors may be used to endorse or promote products derived from
- *       this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- *      Author: Julius Kammerl (jkammerl@willowgarage.com)
- */
-
-#include "openni2_camera/openni2_driver.h"
-
-int main(int argc, char **argv){
-
-  ros::init(argc, argv, "openni2_camera");
-  ros::NodeHandle n;
-  ros::NodeHandle pnh("~");
-
-  openni2_wrapper::OpenNI2Driver drv(n, pnh);
-
-  ros::spin();
-
-  return 0;
-}
diff --git a/openni2_camera/ros/openni2_camera_nodelet.cpp b/openni2_camera/ros/openni2_camera_nodelet.cpp
deleted file mode 100644
index 90f7075..0000000
--- a/openni2_camera/ros/openni2_camera_nodelet.cpp
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * Copyright (c) 2013, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyright
- *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the Willow Garage, Inc. nor the names of its
- *       contributors may be used to endorse or promote products derived from
- *       this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- *      Author: Julius Kammerl (jkammerl@willowgarage.com)
- */
-
-#include "openni2_camera/openni2_driver.h"
-#include <nodelet/nodelet.h>
-
-namespace openni2_camera
-{
-
-class OpenNI2DriverNodelet : public nodelet::Nodelet
-{
-public:
-  OpenNI2DriverNodelet()  {};
-
-  ~OpenNI2DriverNodelet() {}
-
-private:
-  virtual void onInit()
-  {
-    lp.reset(new openni2_wrapper::OpenNI2Driver(getNodeHandle(), getPrivateNodeHandle()));
-  };
-
-  boost::shared_ptr<openni2_wrapper::OpenNI2Driver> lp;
-};
-
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(openni2_camera::OpenNI2DriverNodelet, nodelet::Nodelet)
diff --git a/openni2_camera/src/list_devices.cpp b/openni2_camera/src/list_devices.cpp
index ba3766c..59a6a32 100644
--- a/openni2_camera/src/list_devices.cpp
+++ b/openni2_camera/src/list_devices.cpp
@@ -45,7 +45,7 @@ using openni2_wrapper::OpenNI2Exception;
 int main(int arc, char** argv)
 {
   openni2_wrapper::OpenNI2DeviceManager manager;
-  boost::shared_ptr<std::vector<openni2_wrapper::OpenNI2DeviceInfo> > device_infos = manager.getConnectedDeviceInfos();
+  std::shared_ptr<std::vector<openni2_wrapper::OpenNI2DeviceInfo> > device_infos = manager.getConnectedDeviceInfos();
   std::cout << "Found " << device_infos->size() << " devices:" << std::endl << std::endl;
   for (size_t i = 0; i < device_infos->size(); ++i)
   {
diff --git a/openni2_camera/src/openni2_convert.cpp b/openni2_camera/src/openni2_convert.cpp
index b41de2a..8a102ee 100644
--- a/openni2_camera/src/openni2_convert.cpp
+++ b/openni2_camera/src/openni2_convert.cpp
@@ -32,8 +32,6 @@
 #include "openni2_camera/openni2_convert.h"
 #include "openni2_camera/openni2_exception.h"
 
-#include <boost/make_shared.hpp>
-
 #include <string>
 
 namespace openni2_wrapper
diff --git a/openni2_camera/src/openni2_device.cpp b/openni2_camera/src/openni2_device.cpp
index a2800bc..de4d253 100644
--- a/openni2_camera/src/openni2_device.cpp
+++ b/openni2_camera/src/openni2_device.cpp
@@ -32,22 +32,23 @@
 #include "OpenNI.h"
 #include <PS1080.h> // For XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE property
 
-#include <boost/lexical_cast.hpp>
-#include <boost/algorithm/string/replace.hpp>
-
 #include "openni2_camera/openni2_device.h"
 #include "openni2_camera/openni2_exception.h"
 #include "openni2_camera/openni2_convert.h"
 #include "openni2_camera/openni2_frame_listener.h"
 
-#include <boost/shared_ptr.hpp>
-#include <boost/make_shared.hpp>
-
 #include <string>
 
 namespace openni2_wrapper
 {
 
+void replace_all(std::string &input, const std::string &search, const std::string &replace)
+{
+  size_t pos;
+  while ((pos = input.find(search)) != std::string::npos)
+      input.replace(pos, 1, replace);
+}
+
 OpenNI2Device::OpenNI2Device(const std::string& device_URI,
                              rclcpp::Node* node) :
     openni_device_(),
@@ -61,7 +62,7 @@ OpenNI2Device::OpenNI2Device(const std::string& device_URI,
   if (rc != openni::STATUS_OK)
     THROW_OPENNI_EXCEPTION("Initialize failed\n%s\n", openni::OpenNI::getExtendedError());
 
-  openni_device_ = boost::make_shared<openni::Device>();
+  openni_device_ = std::make_shared<openni::Device>();
 
   if (device_URI.length() > 0)
   {
@@ -75,12 +76,12 @@ OpenNI2Device::OpenNI2Device(const std::string& device_URI,
   if (rc != openni::STATUS_OK)
     THROW_OPENNI_EXCEPTION("Device open failed\n%s\n", openni::OpenNI::getExtendedError());
 
-  device_info_ = boost::make_shared<openni::DeviceInfo>();
+  device_info_ = std::make_shared<openni::DeviceInfo>();
   *device_info_ = openni_device_->getDeviceInfo();
 
-  ir_frame_listener = boost::make_shared<OpenNI2FrameListener>(node);
-  color_frame_listener = boost::make_shared<OpenNI2FrameListener>(node);
-  depth_frame_listener = boost::make_shared<OpenNI2FrameListener>(node);
+  ir_frame_listener = std::make_shared<OpenNI2FrameListener>(node);
+  color_frame_listener = std::make_shared<OpenNI2FrameListener>(node);
+  depth_frame_listener = std::make_shared<OpenNI2FrameListener>(node);
 
 }
 
@@ -122,9 +123,9 @@ const std::string OpenNI2Device::getStringID() const
 {
   std::string ID_str = getName() + "_" + getVendor();
 
-  boost::replace_all(ID_str, "/", "");
-  boost::replace_all(ID_str, ".", "");
-  boost::replace_all(ID_str, "@", "");
+  replace_all(ID_str, "/", "");
+  replace_all(ID_str, ".", "");
+  replace_all(ID_str, "@", "");
 
   return ID_str;
 }
@@ -137,7 +138,7 @@ bool OpenNI2Device::isValid() const
 float OpenNI2Device::getIRFocalLength(int output_y_resolution) const
 {
   float focal_length = 0.0f;
-  boost::shared_ptr<openni::VideoStream> stream = getIRVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getIRVideoStream();
 
   if (stream)
   {
@@ -150,7 +151,7 @@ float OpenNI2Device::getIRFocalLength(int output_y_resolution) const
 float OpenNI2Device::getColorFocalLength(int output_y_resolution) const
 {
   float focal_length = 0.0f;
-  boost::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
 
   if (stream)
   {
@@ -163,7 +164,7 @@ float OpenNI2Device::getColorFocalLength(int output_y_resolution) const
 float OpenNI2Device::getDepthFocalLength(int output_y_resolution) const
 {
   float focal_length = 0.0f;
-  boost::shared_ptr<openni::VideoStream> stream = getDepthVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getDepthVideoStream();
 
   if (stream)
   {
@@ -176,7 +177,7 @@ float OpenNI2Device::getDepthFocalLength(int output_y_resolution) const
 float OpenNI2Device::getBaseline() const
 {
   float baseline = 0.075f;
-  boost::shared_ptr<openni::VideoStream> stream = getDepthVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getDepthVideoStream();
 
   if (stream && stream->isPropertySupported(XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE))
   {
@@ -259,7 +260,7 @@ bool OpenNI2Device::hasDepthSensor() const
 
 void OpenNI2Device::startIRStream()
 {
-  boost::shared_ptr<openni::VideoStream> stream = getIRVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getIRVideoStream();
 
   if (stream)
   {
@@ -273,7 +274,7 @@ void OpenNI2Device::startIRStream()
 
 void OpenNI2Device::startColorStream()
 {
-  boost::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
 
   if (stream)
   {
@@ -285,7 +286,7 @@ void OpenNI2Device::startColorStream()
 }
 void OpenNI2Device::startDepthStream()
 {
-  boost::shared_ptr<openni::VideoStream> stream = getDepthVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getDepthVideoStream();
 
   if (stream)
   {
@@ -365,7 +366,7 @@ bool OpenNI2Device::isDepthStreamStarted()
 
 const std::vector<OpenNI2VideoMode>& OpenNI2Device::getSupportedIRVideoModes() const
 {
-  boost::shared_ptr<openni::VideoStream> stream = getIRVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getIRVideoStream();
 
   ir_video_modes_.clear();
 
@@ -381,7 +382,7 @@ const std::vector<OpenNI2VideoMode>& OpenNI2Device::getSupportedIRVideoModes() c
 
 const std::vector<OpenNI2VideoMode>& OpenNI2Device::getSupportedColorVideoModes() const
 {
-  boost::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
 
   color_video_modes_.clear();
 
@@ -397,7 +398,7 @@ const std::vector<OpenNI2VideoMode>& OpenNI2Device::getSupportedColorVideoModes(
 
 const std::vector<OpenNI2VideoMode>& OpenNI2Device::getSupportedDepthVideoModes() const
 {
-  boost::shared_ptr<openni::VideoStream> stream = getDepthVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getDepthVideoStream();
 
   depth_video_modes_.clear();
 
@@ -447,7 +448,7 @@ const OpenNI2VideoMode OpenNI2Device::getIRVideoMode()
 {
   OpenNI2VideoMode ret;
 
-  boost::shared_ptr<openni::VideoStream> stream = getIRVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getIRVideoStream();
 
   if (stream)
   {
@@ -465,7 +466,7 @@ const OpenNI2VideoMode OpenNI2Device::getColorVideoMode()
 {
   OpenNI2VideoMode ret;
 
-  boost::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
 
   if (stream)
   {
@@ -483,7 +484,7 @@ const OpenNI2VideoMode OpenNI2Device::getDepthVideoMode()
 {
   OpenNI2VideoMode ret;
 
-  boost::shared_ptr<openni::VideoStream> stream = getDepthVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getDepthVideoStream();
 
   if (stream)
   {
@@ -499,7 +500,7 @@ const OpenNI2VideoMode OpenNI2Device::getDepthVideoMode()
 
 void OpenNI2Device::setIRVideoMode(const OpenNI2VideoMode& video_mode)
 {
-  boost::shared_ptr<openni::VideoStream> stream = getIRVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getIRVideoStream();
 
   if (stream)
   {
@@ -512,7 +513,7 @@ void OpenNI2Device::setIRVideoMode(const OpenNI2VideoMode& video_mode)
 
 void OpenNI2Device::setColorVideoMode(const OpenNI2VideoMode& video_mode)
 {
-  boost::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
 
   if (stream)
   {
@@ -525,7 +526,7 @@ void OpenNI2Device::setColorVideoMode(const OpenNI2VideoMode& video_mode)
 
 void OpenNI2Device::setDepthVideoMode(const OpenNI2VideoMode& video_mode)
 {
-  boost::shared_ptr<openni::VideoStream> stream = getDepthVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getDepthVideoStream();
 
   if (stream)
   {
@@ -538,7 +539,7 @@ void OpenNI2Device::setDepthVideoMode(const OpenNI2VideoMode& video_mode)
 
 void OpenNI2Device::setAutoExposure(bool enable)
 {
-  boost::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
 
   if (stream)
   {
@@ -554,7 +555,7 @@ void OpenNI2Device::setAutoExposure(bool enable)
 }
 void OpenNI2Device::setAutoWhiteBalance(bool enable)
 {
-  boost::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
 
   if (stream)
   {
@@ -571,7 +572,7 @@ void OpenNI2Device::setAutoWhiteBalance(bool enable)
 
 void OpenNI2Device::setExposure(int exposure)
 {
-  boost::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
 
   if (stream)
   {
@@ -589,7 +590,7 @@ bool OpenNI2Device::getAutoExposure() const
 {
   bool ret = false;
 
-  boost::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
 
   if (stream)
   {
@@ -605,7 +606,7 @@ bool OpenNI2Device::getAutoWhiteBalance() const
 {
   bool ret = false;
 
-  boost::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
 
   if (stream)
   {
@@ -621,7 +622,7 @@ int OpenNI2Device::getExposure() const
 {
   int ret = 0;
 
-  boost::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
+  std::shared_ptr<openni::VideoStream> stream = getColorVideoStream();
 
   if (stream)
   {
@@ -660,13 +661,13 @@ void OpenNI2Device::setDepthFrameCallback(FrameCallbackFunction callback)
   depth_frame_listener->setCallback(callback);
 }
 
-boost::shared_ptr<openni::VideoStream> OpenNI2Device::getIRVideoStream() const
+std::shared_ptr<openni::VideoStream> OpenNI2Device::getIRVideoStream() const
 {
   if (ir_video_stream_.get() == 0)
   {
     if (hasIRSensor())
     {
-      ir_video_stream_ = boost::make_shared<openni::VideoStream>();
+      ir_video_stream_ = std::make_shared<openni::VideoStream>();
 
       const openni::Status rc = ir_video_stream_->create(*openni_device_, openni::SENSOR_IR);
       if (rc != openni::STATUS_OK)
@@ -676,13 +677,13 @@ boost::shared_ptr<openni::VideoStream> OpenNI2Device::getIRVideoStream() const
   return ir_video_stream_;
 }
 
-boost::shared_ptr<openni::VideoStream> OpenNI2Device::getColorVideoStream() const
+std::shared_ptr<openni::VideoStream> OpenNI2Device::getColorVideoStream() const
 {
   if (color_video_stream_.get() == 0)
   {
     if (hasColorSensor())
     {
-      color_video_stream_ = boost::make_shared<openni::VideoStream>();
+      color_video_stream_ = std::make_shared<openni::VideoStream>();
 
       const openni::Status rc = color_video_stream_->create(*openni_device_, openni::SENSOR_COLOR);
       if (rc != openni::STATUS_OK)
@@ -692,13 +693,13 @@ boost::shared_ptr<openni::VideoStream> OpenNI2Device::getColorVideoStream() cons
   return color_video_stream_;
 }
 
-boost::shared_ptr<openni::VideoStream> OpenNI2Device::getDepthVideoStream() const
+std::shared_ptr<openni::VideoStream> OpenNI2Device::getDepthVideoStream() const
 {
   if (depth_video_stream_.get() == 0)
   {
     if (hasDepthSensor())
     {
-      depth_video_stream_ = boost::make_shared<openni::VideoStream>();
+      depth_video_stream_ = std::make_shared<openni::VideoStream>();
 
       const openni::Status rc = depth_video_stream_->create(*openni_device_, openni::SENSOR_DEPTH);
       if (rc != openni::STATUS_OK)
diff --git a/openni2_camera/src/openni2_device_manager.cpp b/openni2_camera/src/openni2_device_manager.cpp
index ca1459e..f142bdb 100644
--- a/openni2_camera/src/openni2_device_manager.cpp
+++ b/openni2_camera/src/openni2_device_manager.cpp
@@ -34,8 +34,6 @@
 #include "openni2_camera/openni2_device.h"
 #include "openni2_camera/openni2_exception.h"
 
-#include <boost/make_shared.hpp>
-
 #include <rclcpp/rclcpp.hpp>
 
 #include <set>
@@ -108,7 +106,7 @@ class OpenNI2DeviceListener : public openni::OpenNI::DeviceConnectedListener,
 
   virtual void onDeviceConnected(const openni::DeviceInfo* pInfo)
   {
-    boost::mutex::scoped_lock l(device_mutex_);
+    std::lock_guard l(device_mutex_);
 
     const OpenNI2DeviceInfo device_info_wrapped = openni2_convert(pInfo);
 
@@ -122,7 +120,7 @@ class OpenNI2DeviceListener : public openni::OpenNI::DeviceConnectedListener,
 
   virtual void onDeviceDisconnected(const openni::DeviceInfo* pInfo)
   {
-    boost::mutex::scoped_lock l(device_mutex_);
+    std::lock_guard l(device_mutex_);
 
     RCLCPP_WARN(rclcpp::get_logger("openni2"), "Device \"%s\" disconnected\n", pInfo->getUri());
 
@@ -130,11 +128,11 @@ class OpenNI2DeviceListener : public openni::OpenNI::DeviceConnectedListener,
     device_set_.erase(device_info_wrapped);
   }
 
-  boost::shared_ptr<std::vector<std::string> > getConnectedDeviceURIs()
+  std::shared_ptr<std::vector<std::string> > getConnectedDeviceURIs()
   {
-    boost::mutex::scoped_lock l(device_mutex_);
+    std::lock_guard l(device_mutex_);
 
-    boost::shared_ptr<std::vector<std::string> > result = boost::make_shared<std::vector<std::string> >();
+    std::shared_ptr<std::vector<std::string> > result = std::make_shared<std::vector<std::string> >();
 
     result->reserve(device_set_.size());
 
@@ -147,11 +145,11 @@ class OpenNI2DeviceListener : public openni::OpenNI::DeviceConnectedListener,
     return result;
   }
 
-  boost::shared_ptr<std::vector<OpenNI2DeviceInfo> > getConnectedDeviceInfos()
+  std::shared_ptr<std::vector<OpenNI2DeviceInfo> > getConnectedDeviceInfos()
   {
-    boost::mutex::scoped_lock l(device_mutex_);
+    std::lock_guard l(device_mutex_);
 
-    boost::shared_ptr<std::vector<OpenNI2DeviceInfo> > result = boost::make_shared<std::vector<OpenNI2DeviceInfo> >();
+    std::shared_ptr<std::vector<OpenNI2DeviceInfo> > result = std::make_shared<std::vector<OpenNI2DeviceInfo> >();
 
     result->reserve(device_set_.size());
 
@@ -166,18 +164,18 @@ class OpenNI2DeviceListener : public openni::OpenNI::DeviceConnectedListener,
 
   std::size_t getNumOfConnectedDevices()
   {
-    boost::mutex::scoped_lock l(device_mutex_);
+    std::lock_guard l(device_mutex_);
 
     return device_set_.size();
   }
 
-  boost::mutex device_mutex_;
+  std::mutex device_mutex_;
   DeviceSet device_set_;
 };
 
 //////////////////////////////////////////////////////////////////////////
 
-boost::shared_ptr<OpenNI2DeviceManager> OpenNI2DeviceManager::singelton_;
+std::shared_ptr<OpenNI2DeviceManager> OpenNI2DeviceManager::singelton_;
 
 OpenNI2DeviceManager::OpenNI2DeviceManager()
 {
@@ -185,27 +183,27 @@ OpenNI2DeviceManager::OpenNI2DeviceManager()
   if (rc != openni::STATUS_OK)
       THROW_OPENNI_EXCEPTION("Initialize failed\n%s\n", openni::OpenNI::getExtendedError());
 
-  device_listener_ = boost::make_shared<OpenNI2DeviceListener>();
+  device_listener_ = std::make_shared<OpenNI2DeviceListener>();
 }
 
 OpenNI2DeviceManager::~OpenNI2DeviceManager()
 {
 }
 
-boost::shared_ptr<OpenNI2DeviceManager> OpenNI2DeviceManager::getSingelton()
+std::shared_ptr<OpenNI2DeviceManager> OpenNI2DeviceManager::getSingelton()
 {
   if (singelton_.get()==0)
-    singelton_ = boost::make_shared<OpenNI2DeviceManager>();
+    singelton_ = std::make_shared<OpenNI2DeviceManager>();
 
   return singelton_;
 }
 
-boost::shared_ptr<std::vector<OpenNI2DeviceInfo> > OpenNI2DeviceManager::getConnectedDeviceInfos() const
+std::shared_ptr<std::vector<OpenNI2DeviceInfo> > OpenNI2DeviceManager::getConnectedDeviceInfos() const
 {
 return device_listener_->getConnectedDeviceInfos();
 }
 
-boost::shared_ptr<std::vector<std::string> > OpenNI2DeviceManager::getConnectedDeviceURIs() const
+std::shared_ptr<std::vector<std::string> > OpenNI2DeviceManager::getConnectedDeviceURIs() const
 {
   return device_listener_->getConnectedDeviceURIs();
 }
@@ -243,19 +241,19 @@ std::string OpenNI2DeviceManager::getSerial(const std::string& Uri) const
   return ret;
 }
 
-boost::shared_ptr<OpenNI2Device> OpenNI2DeviceManager::getAnyDevice(rclcpp::Node* node)
+std::shared_ptr<OpenNI2Device> OpenNI2DeviceManager::getAnyDevice(rclcpp::Node* node)
 {
-  return boost::make_shared<OpenNI2Device>("", node);
+  return std::make_shared<OpenNI2Device>("", node);
 }
-boost::shared_ptr<OpenNI2Device> OpenNI2DeviceManager::getDevice(const std::string& device_URI, rclcpp::Node* node)
+std::shared_ptr<OpenNI2Device> OpenNI2DeviceManager::getDevice(const std::string& device_URI, rclcpp::Node* node)
 {
-  return boost::make_shared<OpenNI2Device>(device_URI, node);
+  return std::make_shared<OpenNI2Device>(device_URI, node);
 }
 
 
 std::ostream& operator << (std::ostream& stream, const OpenNI2DeviceManager& device_manager) {
 
-  boost::shared_ptr<std::vector<OpenNI2DeviceInfo> > device_info = device_manager.getConnectedDeviceInfos();
+  std::shared_ptr<std::vector<OpenNI2DeviceInfo> > device_info = device_manager.getConnectedDeviceInfos();
 
   std::vector<OpenNI2DeviceInfo>::const_iterator it;
   std::vector<OpenNI2DeviceInfo>::const_iterator it_end = device_info->end();
diff --git a/openni2_camera/src/openni2_driver.cpp b/openni2_camera/src/openni2_driver.cpp
index d74c539..8ae99c2 100644
--- a/openni2_camera/src/openni2_driver.cpp
+++ b/openni2_camera/src/openni2_driver.cpp
@@ -35,12 +35,12 @@
 #include <sensor_msgs/image_encodings.hpp>
 #include <sensor_msgs/distortion_models.hpp>
 
-#include <boost/date_time/posix_time/posix_time.hpp>
-#include <boost/thread/thread.hpp>
-
 namespace openni2_wrapper
 {
 
+using namespace std::placeholders;
+using namespace std::chrono_literals;
+
 OpenNI2Driver::OpenNI2Driver(const rclcpp::NodeOptions & node_options) :
     Node("openni2_camera", node_options),
     device_manager_(OpenNI2DeviceManager::getSingelton()),
@@ -134,6 +134,7 @@ void OpenNI2Driver::periodic()
     applyConfigToOpenNIDevice();
 
     // Register parameter callback
+    parameters_callback_ = \
     this->add_on_set_parameters_callback(std::bind(&OpenNI2Driver::paramCb, this, std::placeholders::_1));
     initialized_ = true;
   }
@@ -156,7 +157,7 @@ void OpenNI2Driver::advertiseROSTopics()
   // connectCb() can fire while we're advertising (say) "depth/image_raw", but before we actually
   // assign to pub_depth_raw_. Then pub_depth_raw_.getNumSubscribers() returns 0, and we fail to start
   // the depth generator.
-  boost::lock_guard<boost::mutex> lock(connect_mutex_);
+  std::lock_guard<std::mutex> lock(connect_mutex_);
 
   // Asus Xtion PRO does not have an RGB camera
   if (device_->hasColorSensor())
@@ -373,7 +374,7 @@ void OpenNI2Driver::colorConnectCb()
     RCLCPP_WARN_STREAM(this->get_logger(), "Callback in " << __FUNCTION__ <<  "failed due to null device");
     return;
   }
-  boost::lock_guard<boost::mutex> lock(connect_mutex_);
+  std::lock_guard<std::mutex> lock(connect_mutex_);
 
   // This does not appear to work
   color_subscribers_ = pub_color_.getNumSubscribers() > 0;
@@ -388,7 +389,7 @@ void OpenNI2Driver::colorConnectCb()
       device_->stopIRStream();
     }
 
-    device_->setColorFrameCallback(boost::bind(&OpenNI2Driver::newColorFrameCallback, this, _1));
+    device_->setColorFrameCallback(std::bind(&OpenNI2Driver::newColorFrameCallback, this, _1));
 
     RCLCPP_INFO(this->get_logger(), "Starting color stream.");
     device_->startColorStream();
@@ -398,7 +399,7 @@ void OpenNI2Driver::colorConnectCb()
     {
       RCLCPP_INFO_STREAM(this->get_logger(), "Exposure is set to " << exposure_ << ", forcing on color stream start");
         //delay for stream to start, before setting exposure
-      boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+      std::this_thread::sleep_for(100ms);
       forceSetExposure();
     }
   }
@@ -411,7 +412,7 @@ void OpenNI2Driver::colorConnectCb()
     bool need_ir = pub_ir_.getNumSubscribers() > 0;
     if (need_ir && !device_->isIRStreamStarted())
     {
-      device_->setIRFrameCallback(boost::bind(&OpenNI2Driver::newIRFrameCallback, this, _1));
+      device_->setIRFrameCallback(std::bind(&OpenNI2Driver::newIRFrameCallback, this, _1));
 
       RCLCPP_INFO(this->get_logger(), "Starting IR stream.");
       device_->startIRStream();
@@ -426,7 +427,7 @@ void OpenNI2Driver::depthConnectCb()
     RCLCPP_WARN_STREAM(this->get_logger(), "Callback in " << __FUNCTION__ <<  "failed due to null device");
     return;
   }
-  boost::lock_guard<boost::mutex> lock(connect_mutex_);
+  std::lock_guard<std::mutex> lock(connect_mutex_);
 
   // These does not appear to work
   depth_subscribers_ = pub_depth_.getNumSubscribers() > 0;
@@ -437,7 +438,7 @@ void OpenNI2Driver::depthConnectCb()
 
   if (need_depth && !device_->isDepthStreamStarted())
   {
-    device_->setDepthFrameCallback(boost::bind(&OpenNI2Driver::newDepthFrameCallback, this, _1));
+    device_->setDepthFrameCallback(std::bind(&OpenNI2Driver::newDepthFrameCallback, this, _1));
 
     RCLCPP_INFO(this->get_logger(), "Starting depth stream.");
     device_->startDepthStream();
@@ -456,7 +457,7 @@ void OpenNI2Driver::irConnectCb()
     RCLCPP_WARN_STREAM(this->get_logger(), "Callback in " << __FUNCTION__ <<  "failed due to null device");
     return;
   }
-  boost::lock_guard<boost::mutex> lock(connect_mutex_);
+  std::lock_guard<std::mutex> lock(connect_mutex_);
 
   // This does not appear to work
   // ir_subscribers_ = pub_ir_.getNumSubscribers() > 0;
@@ -472,7 +473,7 @@ void OpenNI2Driver::irConnectCb()
     }
     else
     {
-      device_->setIRFrameCallback(boost::bind(&OpenNI2Driver::newIRFrameCallback, this, _1));
+      device_->setIRFrameCallback(std::bind(&OpenNI2Driver::newIRFrameCallback, this, _1));
 
       RCLCPP_INFO(this->get_logger(), "Starting IR stream.");
       device_->startIRStream();
@@ -718,7 +719,7 @@ std::string OpenNI2Driver::resolveDeviceURI(const std::string& device_id)
 {
   // retrieve available device URIs, they look like this: "1d27/0601@1/5"
   // which is <vendor ID>/<product ID>@<bus number>/<device number>
-  boost::shared_ptr<std::vector<std::string> > available_device_URIs =
+  std::shared_ptr<std::vector<std::string> > available_device_URIs =
     device_manager_->getConnectedDeviceURIs();
 
   // look for '#<number>' format
@@ -842,7 +843,7 @@ void OpenNI2Driver::initDevice()
       if (!device_)
       {
         RCLCPP_INFO(this->get_logger(), "No matching device found.... waiting for devices. Reason: %s", exception.what());
-        boost::this_thread::sleep(boost::posix_time::seconds(3));
+        std::this_thread::sleep_for(3s);
         continue;
       }
       else
@@ -856,7 +857,7 @@ void OpenNI2Driver::initDevice()
   while (rclcpp::ok() && !device_->isValid())
   {
     RCLCPP_ERROR(this->get_logger(), "Waiting for device initialization..");
-    boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+    std::this_thread::sleep_for(100ms);
   }
 }
 
@@ -875,7 +876,7 @@ bool OpenNI2Driver::isConnected() const
   // TODO: The current isConnected logic assumes that there is only one sensor
   // on the bus of interest.  In the future, we could compare serial numbers
   // to make certain the same camera as been re-connected.
-  boost::shared_ptr<std::vector<std::string> > list =
+  std::shared_ptr<std::vector<std::string> > list =
       device_manager_->getConnectedDeviceURIs();
   for (std::size_t i = 0; i != list->size(); ++i)
   {
@@ -900,14 +901,14 @@ void OpenNI2Driver::monitorConnection()
       try
       {
         {
-        boost::lock_guard<boost::mutex> lock(connect_mutex_);
+        std::lock_guard<std::mutex> lock(connect_mutex_);
         std::string device_URI = resolveDeviceURI(device_id_);
         device_ = device_manager_->getDevice(device_URI, this);
         bus_id_ = extractBusID(device_->getUri() );
         while (rclcpp::ok() && !device_->isValid())
         {
           RCLCPP_INFO(this->get_logger(), "Waiting for device initialization, before configuring and restarting publishers");
-          boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+          std::this_thread::sleep_for(100ms);
         }
         }
         RCLCPP_INFO_STREAM(this->get_logger(), "Re-applying configuration to camera on re-init");
@@ -934,7 +935,7 @@ void OpenNI2Driver::monitorConnection()
           device_->setAutoWhiteBalance(true);
           RCLCPP_INFO_STREAM(this->get_logger(), "Waiting for color camera to come up and adjust");
           // It takes about 2.5 seconds for the camera to adjust
-          boost::this_thread::sleep(boost::posix_time::milliseconds(2500));
+          std::this_thread::sleep_for(2500ms);
           RCLCPP_WARN_STREAM(this->get_logger(), "Resetting auto exposure and white balance to previous values");
           device_->setAutoExposure(auto_exposure_);
           device_->setAutoWhiteBalance(auto_white_balance_);
diff --git a/openni2_camera/test/test_wrapper.cpp b/openni2_camera/test/test_wrapper.cpp
index 95b9d18..6a32c91 100644
--- a/openni2_camera/test/test_wrapper.cpp
+++ b/openni2_camera/test/test_wrapper.cpp
@@ -32,10 +32,6 @@
 #include "openni2_camera/openni2_device_manager.h"
 #include "openni2_camera/openni2_device.h"
 
-#include <boost/date_time/posix_time/posix_time.hpp>
-#include <boost/thread/thread.hpp>
-#include <boost/foreach.hpp>
-
 #include <iostream>
 
 using namespace std;
@@ -66,17 +62,17 @@ int main()
 
   std::cout << device_manager;
 
-  boost::shared_ptr<std::vector<std::string> > device_uris = device_manager.getConnectedDeviceURIs();
+  std::shared_ptr<std::vector<std::string> > device_uris = device_manager.getConnectedDeviceURIs();
 
-  BOOST_FOREACH(const std::string& uri, *device_uris)
+  for(const std::string& uri : *device_uris)
   {
-    boost::shared_ptr<OpenNI2Device> device = device_manager.getDevice(uri);
+    std::shared_ptr<OpenNI2Device> device = device_manager.getDevice(uri);
 
     std::cout << *device;
 
-    device->setIRFrameCallback(boost::bind(&IRCallback, _1));
-    device->setColorFrameCallback(boost::bind(&ColorCallback, _1));
-    device->setDepthFrameCallback(boost::bind(&DepthCallback, _1));
+    device->setIRFrameCallback(std::bind(&IRCallback, _1));
+    device->setColorFrameCallback(std::bind(&ColorCallback, _1));
+    device->setDepthFrameCallback(std::bind(&DepthCallback, _1));
 
     ir_counter_ = 0;
     color_counter_ = 0;
@@ -85,7 +81,7 @@ int main()
     device->startColorStream();
     device->startDepthStream();
 
-    boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+    std::this_thread::sleep_for(1000ms);
 
     device->stopAllStreams();