diff --git a/image_proc/CMakeLists.txt b/image_proc/CMakeLists.txt
index c11a7d071..6b74b41a4 100644
--- a/image_proc/CMakeLists.txt
+++ b/image_proc/CMakeLists.txt
@@ -110,6 +110,14 @@ install(TARGETS image_proc_exe
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+
+  add_definitions(-D_SRC_RESOURCES_DIR_PATH="${CMAKE_CURRENT_SOURCE_DIR}/test/resources")
+
+  find_package(ament_cmake_gtest)
+  ament_auto_add_gtest(test_rectify test/test_rectify.cpp)
+
+  find_package(ament_cmake_gtest)
+  ament_auto_add_gtest(rostest test/rostest.cpp)
 endif()
 
 ament_auto_package(INSTALL_TO_SHARE launch)
diff --git a/image_proc/package.xml b/image_proc/package.xml
index a54db2bbb..f79d1396f 100644
--- a/image_proc/package.xml
+++ b/image_proc/package.xml
@@ -20,6 +20,7 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
+  <depend>camera_calibration_parsers</depend>
   <depend>cv_bridge</depend>
   <depend>image_geometry</depend>
   <depend>image_transport</depend>
diff --git a/image_proc/test/CMakeLists.txt b/image_proc/test/CMakeLists.txt
deleted file mode 100644
index 52fa28569..000000000
--- a/image_proc/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-find_package(rostest REQUIRED)
-#catkin_add_gtest(image_proc_rostest rostest.cpp)
-#target_link_libraries(image_proc_rostest ${catkin_LIBRARIES}  ${Boost_LIBRARIES})
-add_rostest_gtest(image_proc_test_rectify test_rectify.xml test_rectify.cpp)
-target_link_libraries(image_proc_test_rectify ${catkin_LIBRARIES})
diff --git a/image_proc/test/resources/calibration_file.ini b/image_proc/test/resources/calibration_file.ini
new file mode 100644
index 000000000..37485c4ab
--- /dev/null
+++ b/image_proc/test/resources/calibration_file.ini
@@ -0,0 +1,29 @@
+[image]
+
+#comment
+width
+640
+
+; comment
+height
+480
+
+[mono_left]
+
+camera matrix
+369.344588 0.000000 320.739078
+0.000000 367.154330 203.592450
+0.000000 0.000000 1.000000
+
+distortion
+0.189544 -0.018229 -0.000630 0.000054 -0.000212
+
+rectification
+1.000000 0.000000 0.000000
+0.000000 1.000000 0.000000
+0.000000 0.000000 1.000000
+
+projection
+262.927429 0.000000 320.984481 0.000000
+0.000000 302.056213 188.592437 0.000000
+0.000000 0.000000 1.000000 0.000000
diff --git a/image_proc/test/resources/logo.png b/image_proc/test/resources/logo.png
new file mode 100644
index 000000000..6c38d3e3a
Binary files /dev/null and b/image_proc/test/resources/logo.png differ
diff --git a/image_proc/test/rostest.cpp b/image_proc/test/rostest.cpp
index 32cb9214d..f84594af0 100644
--- a/image_proc/test/rostest.cpp
+++ b/image_proc/test/rostest.cpp
@@ -30,17 +30,19 @@
 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
+#include <gtest/gtest.h>
+
+#include <memory>
 #include <string>
 
-#include "ros/ros.h>"
-#include "gtest/gtest.h"
-#include "cv_bridge/cv_bridge.hpp"
+#include <rclcpp/rclcpp.hpp>
+#include <cv_bridge/cv_bridge.hpp>
 
 #include <camera_calibration_parsers/parse.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <image_transport/image_transport.hpp>
 
-#include <boost/foreach.hpp>
+using std::placeholders::_1;
 
 class ImageProcTest
   : public testing::Test
@@ -48,10 +50,10 @@ class ImageProcTest
 protected:
   virtual void SetUp()
   {
-    ros::NodeHandle local_nh("~");
+    node = rclcpp::Node::make_shared("image_proc_test");
 
     // Determine topic names
-    std::string camera_ns = nh.resolveName("camera") + "/";
+    std::string camera_ns = node->get_node_topics_interface()->resolve_topic_name("camera") + "/";
 
     if (camera_ns == "/camera") {
       throw "Must remap 'camera' to the camera namespace.";
@@ -63,84 +65,79 @@ class ImageProcTest
     topic_color = camera_ns + "image_color";
     topic_rect_color = camera_ns + "image_rect_color";
 
-    // Load raw image and cam info
-    /// @todo Make these cmd-line args instead?
-    std::string raw_image_file, cam_info_file;
-
-    if (!local_nh.getParam("raw_image_file", raw_image_file)) {
-      throw "Must set parameter ~raw_image_file.";
-    }
-
-    if (!local_nh.getParam("camera_info_file", cam_info_file)) {
-      throw "Must set parameter ~camera_info_file.";
-    }
+    const rcpputils::fs::path base{_SRC_RESOURCES_DIR_PATH};
+    const rcpputils::fs::path raw_image_file = base / "logo.png";
+    const rcpputils::fs::path cam_info_file = base / "calibration_file.ini";
 
     /// @todo Test variety of encodings for raw image (bayer, mono, color)
-    cv::Mat img = cv::imread(raw_image_file, 0);
-    raw_image = cv_bridge::CvImage(std_msgs::Header(), "mono8", img).toImageMsg();
+    cv::Mat img = cv::imread(raw_image_file.string(), 0);
+    raw_image = cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", img).toImageMsg();
     std::string cam_name;
 
-    if (!camera_calibration_parsers::readCalibration(cam_info_file, cam_name, cam_info)) {
+    if (!camera_calibration_parsers::readCalibration(cam_info_file.string(), cam_name, cam_info)) {
       throw "Failed to read camera info file.";
     }
 
     // Create raw camera publisher
-    image_transport::ImageTransport it(nh);
+    image_transport::ImageTransport it(this->node);
     cam_pub = it.advertiseCamera(topic_raw, 1);
 
-    // Wait for image_proc to be operational
-    ros::master::V_TopicInfo topics;
     while (true) {
-      if (ros::master::getTopics(topics)) {
-        BOOST_FOREACH(ros::master::TopicInfo & topic, topics) {
-          if (topic.name == topic_rect_color) {
-            return;
-          }
+      // Wait for image_proc to be operational
+      auto topic_names_and_types = this->node->get_topic_names_and_types();
+      for (const auto & map_pair : topic_names_and_types) {
+        if (map_pair.first == topic_raw) {
+          return;
         }
       }
-
-      ros::Duration(0.5).sleep();
     }
   }
 
-  ros::NodeHandle nh;
+  rclcpp::Node::SharedPtr node;
   std::string topic_raw;
   std::string topic_mono;
   std::string topic_rect;
   std::string topic_color;
   std::string topic_rect_color;
 
-  sensor_msgs::ImagePtr raw_image;
-  sensor_msgs::CameraInfo cam_info;
+  sensor_msgs::msg::Image::SharedPtr raw_image;
+  sensor_msgs::msg::CameraInfo cam_info;
   image_transport::CameraPublisher cam_pub;
 
   void publishRaw()
   {
     cam_pub.publish(*raw_image, cam_info);
   }
-};
 
-void callback(const sensor_msgs::ImageConstPtr & msg)
-{
-  ROS_FATAL("Got an image");
-  ros::shutdown();
-}
+public:
+  bool has_new_image_{false};
+  void callback(const sensor_msgs::msg::Image::ConstSharedPtr & /*msg*/)
+  {
+    RCLCPP_INFO(node->get_logger(), "Got an image");
+    has_new_image_ = true;
+  }
+};
 
 TEST_F(ImageProcTest, monoSubscription)
 {
-  ROS_INFO("In test. Subscribing.");
-  ros::Subscriber mono_sub = nh.subscribe(topic_mono, 1, callback);
-  ROS_INFO("Publishing.");
-  publishRaw();
-
-  ROS_INFO("Spinning.");
-  ros::spin();
-  ROS_INFO("Done.");
+  RCLCPP_INFO(node->get_logger(), "In test. Subscribing.");
+  auto mono_sub = node->create_subscription<sensor_msgs::msg::Image>(
+    topic_raw, 1, std::bind(&ImageProcTest::callback, this, _1));
+
+  RCLCPP_INFO(node->get_logger(), "Publishing");
+
+  RCLCPP_INFO(node->get_logger(), "Spinning");
+  while (!has_new_image_) {
+    publishRaw();
+    rclcpp::spin_some(node);
+  }
+  rclcpp::shutdown();
+  RCLCPP_INFO(node->get_logger(), "Done");
 }
 
 int main(int argc, char ** argv)
 {
-  ros::init(argc, argv, "imageproc_rostest");
+  rclcpp::init(argc, argv);
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();
 }
diff --git a/image_proc/test/test_bayer.xml b/image_proc/test/test_bayer.xml
deleted file mode 100644
index a6d2a1a34..000000000
--- a/image_proc/test/test_bayer.xml
+++ /dev/null
@@ -1,10 +0,0 @@
-<launch>
-  <node name="forearm_image_proc" pkg="image_proc" type="image_proc">
-    <remap from="camera" to="/forearm_camera_r"/>
-  </node>
-  <test test-name="test_bayer" pkg="image_proc" type="image_proc_rostest">
-    <remap from="camera" to="/forearm_camera_r"/>
-    <param name="raw_image_file" value="/u/mihelich/ros/vision_opencv/image_geometry/test/forearm_screenshot_3.png" />
-    <param name="camera_info_file" value="/u/mihelich/ros/vision_opencv/image_geometry/test/forearm.ini" />
-  </test>
-</launch>
diff --git a/image_proc/test/test_rectify.cpp b/image_proc/test/test_rectify.cpp
index 2566f4409..adf3dedaa 100644
--- a/image_proc/test/test_rectify.cpp
+++ b/image_proc/test/test_rectify.cpp
@@ -30,18 +30,25 @@
 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
+#include <gtest/gtest.h>
+
 #include <algorithm>
+#include <chrono>
 #include <string>
+#include <thread>
 
-#include "ros/ros.h"
-#include "gtest/gtest.h"
-#include "cv_bridge/cv_bridge.hpp"
+#include <rclcpp/rclcpp.hpp>
+#include <cv_bridge/cv_bridge.hpp>
 
 #include <camera_calibration_parsers/parse.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <image_transport/image_transport.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
-#include <sensor_msgs/msg/distortion_models.hpp>
+#include <sensor_msgs/distortion_models.hpp>
+
+#include "image_proc/rectify.hpp"
+
+using namespace std::chrono_literals;
 
 class ImageProcRectifyTest
   : public testing::Test
@@ -49,8 +56,9 @@ class ImageProcRectifyTest
 protected:
   virtual void SetUp()
   {
+    node = rclcpp::Node::make_shared("test_rectify_node_test");
     // Determine topic names
-    std::string camera_ns = nh_.resolveName("camera") + "/";
+    std::string camera_ns = node->get_node_topics_interface()->resolve_topic_name("camera") + "/";
 
     if (camera_ns == "/camera") {
       throw "Must remap 'camera' to the camera namespace.";
@@ -94,11 +102,11 @@ class ImageProcRectifyTest
     cam_info_.height = 480;
     cam_info_.width = 640;
     // No ROI
-    cam_info_.D.resize(5);
-    std::copy(D, D + 5, cam_info_.D.begin());
-    std::copy(K, K + 9, cam_info_.K.begin());
-    std::copy(R, R + 9, cam_info_.R.begin());
-    std::copy(P, P + 12, cam_info_.P.begin());
+    cam_info_.d.resize(5);
+    std::copy(D, D + 5, cam_info_.d.begin());
+    std::copy(K, K + 9, cam_info_.k.begin());
+    std::copy(R, R + 9, cam_info_.r.begin());
+    std::copy(P, P + 12, cam_info_.p.begin());
     cam_info_.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
 
     distorted_image_ = cv::Mat(cv::Size(cam_info_.width, cam_info_.height), CV_8UC3);
@@ -122,14 +130,28 @@ class ImageProcRectifyTest
     }
 
     raw_image_ = cv_bridge::CvImage(
-      std_msgs::Header(), "bgr8", distorted_image_).toImageMsg();
+      std_msgs::msg::Header(), "bgr8", distorted_image_).toImageMsg();
+
+    rclcpp::NodeOptions options;
+    options.arguments(
+    {
+      "--ros-args", "-r", std::string("__ns:=") + "/camera",
+      "-r", "/camera/image:=/camera/image_raw"});
+    node_rectify = std::make_shared<image_proc::RectifyNode>(options);
+
+    spin_rectify_thread = std::thread(
+      [this]() {
+        rclcpp::spin(node_rectify);
+      });
 
     // Create raw camera subscriber and publisher
-    image_transport::ImageTransport it(nh_);
+    image_transport::ImageTransport it(node);
     cam_pub_ = it.advertiseCamera(topic_raw_, 1);
   }
 
-  ros::NodeHandle nh_;
+  rclcpp::Node::SharedPtr node;
+  rclcpp::Node::SharedPtr node_rectify;
+  std::thread spin_rectify_thread;
   std::string topic_raw_;
   std::string topic_mono_;
   std::string topic_rect_;
@@ -137,22 +159,22 @@ class ImageProcRectifyTest
   std::string topic_rect_color_;
 
   cv::Mat distorted_image_;
-  sensor_msgs::ImagePtr raw_image_;
+  sensor_msgs::msg::Image::SharedPtr raw_image_;
   bool has_new_image_;
   cv::Mat received_image_;
-  sensor_msgs::CameraInfo cam_info_;
+  sensor_msgs::msg::CameraInfo cam_info_;
   image_transport::CameraPublisher cam_pub_;
   image_transport::Subscriber cam_sub_;
 
 public:
-  void imageCallback(const sensor_msgs::ImageConstPtr & msg)
+  void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg)
   {
     cv_bridge::CvImageConstPtr cv_ptr;
 
     try {
       cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8);
     } catch (cv_bridge::Exception & e) {
-      ROS_FATAL("cv_bridge exception: %s", e.what());
+      RCLCPP_ERROR(node->get_logger(), "cv_bridge exception: '%s'", e.what());
       return;
     }
 
@@ -163,21 +185,26 @@ class ImageProcRectifyTest
   void publishRaw()
   {
     has_new_image_ = false;
+    raw_image_->header.stamp = this->node->now();
+    cam_info_.header.stamp = raw_image_->header.stamp;
     cam_pub_.publish(*raw_image_, cam_info_);
   }
 };
 
 TEST_F(ImageProcRectifyTest, rectifyTest)
 {
-  ROS_INFO("In test. Subscribing.");
-  image_transport::ImageTransport it(nh_);
+  RCLCPP_INFO(node->get_logger(), "In test. Subscribing.");
+  image_transport::ImageTransport it(node);
   cam_sub_ = it.subscribe(
-    topic_rect_, 1, &ImageProcRectifyTest::imageCallback,
+    topic_rect_, rclcpp::SensorDataQoS().get_rmw_qos_profile(),
+    &ImageProcRectifyTest::imageCallback,
     dynamic_cast<ImageProcRectifyTest *>(this));
 
   // Wait for image_proc to be operational
   bool wait_for_topic = true;
 
+  rclcpp::WallRate loop_rate(500ms);
+
   while (wait_for_topic) {
     // @todo this fails without the additional 0.5 second sleep after the
     // publisher comes online, which means on a slower or more heavily
@@ -186,8 +213,7 @@ TEST_F(ImageProcRectifyTest, rectifyTest)
     if (cam_sub_.getNumPublishers() > 0) {
       wait_for_topic = false;
     }
-
-    ros::Duration(0.5).sleep();
+    loop_rate.sleep();
   }
 
   // All the tests are the same as from
@@ -203,8 +229,8 @@ TEST_F(ImageProcRectifyTest, rectifyTest)
   // use original cam_info
   publishRaw();
   while (!has_new_image_) {
-    ros::spinOnce();
-    ros::Duration(0.5).sleep();
+    rclcpp::spin_some(node);
+    loop_rate.sleep();
   }
 
   // Test that rectified image is sufficiently different
@@ -216,37 +242,37 @@ TEST_F(ImageProcRectifyTest, rectifyTest)
   // Test that rectified image is sufficiently different
   // using default distortion but with first element zeroed
   // out.
-  sensor_msgs::CameraInfo cam_info_orig = cam_info_;
-  cam_info_.D[0] = 0.0;
+  sensor_msgs::msg::CameraInfo cam_info_orig = cam_info_;
+  cam_info_.d[0] = 0.0;
   publishRaw();
 
   while (!has_new_image_) {
-    ros::spinOnce();
-    ros::Duration(0.5).sleep();
+    rclcpp::spin_some(node);
+    loop_rate.sleep();
   }
 
   error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
   EXPECT_GT(error, diff_threshold);
 
   // Test that rectified image is the same using zero distortion
-  cam_info_.D.assign(cam_info_.D.size(), 0);
+  cam_info_.d.assign(cam_info_.d.size(), 0);
   publishRaw();
 
   while (!has_new_image_) {
-    ros::spinOnce();
-    ros::Duration(0.5).sleep();
+    rclcpp::spin_some(node);
+    loop_rate.sleep();
   }
 
   error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
   EXPECT_EQ(error, 0);
 
   // Test that rectified image is the same using empty distortion
-  cam_info_.D.clear();
+  cam_info_.d.clear();
   publishRaw();
 
   while (!has_new_image_) {
-    ros::spinOnce();
-    ros::Duration(0.5).sleep();
+    rclcpp::spin_some(node);
+    loop_rate.sleep();
   }
 
   error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
@@ -255,11 +281,14 @@ TEST_F(ImageProcRectifyTest, rectifyTest)
 
   // restore the original cam_info for other tests added in the future
   cam_info_ = cam_info_orig;
+
+  rclcpp::shutdown();
+  spin_rectify_thread.join();
 }
 
 int main(int argc, char ** argv)
 {
-  ros::init(argc, argv, "image_proc_test_rectify");
+  rclcpp::init(argc, argv);
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();
 }
diff --git a/image_proc/test/test_rectify.xml b/image_proc/test/test_rectify.xml
deleted file mode 100644
index f1a451c32..000000000
--- a/image_proc/test/test_rectify.xml
+++ /dev/null
@@ -1,9 +0,0 @@
-<launch>
-  <group ns="camera">
-  <node name="rectify_image_proc" pkg="image_proc" type="image_proc"
-      output="screen">
-  </node>
-  </group>
-  <test test-name="image_proc_test_rectify" pkg="image_proc" type="image_proc_test_rectify">
-  </test>
-</launch>