Port image_proc test to ROS 2

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>

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)

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>

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

image_proc/test/rostest.cpp

@@ -30,28 +30,29 @@
 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
+#include <memory>
 #include <string>
 
-#include "ros/ros.h>"
-#include "gtest/gtest.h"
-#include "cv_bridge/cv_bridge.hpp"
+#include <rclcpp/rclcpp.hpp>
+#include <gtest/gtest.h>
+#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
 {
 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 +64,78 @@ 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);
   }
+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;
+  }
 };
 
-void callback(const sensor_msgs::ImageConstPtr & msg)
-{
-  ROS_FATAL("Got an image");
-  ros::shutdown();
-}
-
 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();
 }