Port auto_init_403 test to ROS2

fiducial_slam/CMakeLists.txt

@@ -86,12 +86,29 @@ install(FILES fiducials.rviz
 ###########
 
 if(BUILD_TESTING)
+
   find_package(ament_cmake_gtest REQUIRED)
+  find_package(ros_testing REQUIRED)
+
+  install(DIRECTORY test/test_images
+    DESTINATION share/${PROJECT_NAME}/test
+  )
+
+  # transform_var_test
   ament_add_gtest(transform_var_test
                   test/transform_var_test.cpp
                   src/transform_with_variance.cpp)
   ament_target_dependencies(transform_var_test
                             "geometry_msgs")
+
+  # auto_init_403_test
+  ament_add_gtest_executable(
+    auto_init_403_test
+    test/auto_init_403_test.cpp
+  )
+  target_link_libraries(auto_init_403_test)
+  ament_target_dependencies(auto_init_403_test ${DEPENDENCIES})
+  add_ros_test(test/auto_init_403.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
 endif()
 # if(CATKIN_ENABLE_TESTING)
 #         find_package(catkin REQUIRED COMPONENTS

fiducial_slam/package.xml

@@ -27,6 +27,9 @@
   <exec_depend>rosidl_default_runtime</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>
+
+  <test_depend>ros_testing</test_depend>
+  <test_depend>ament_cmake_test</test_depend>
   <member_of_group>rosidl_interface_packages</member_of_group>
   <export>
     <build_type>ament_cmake</build_type>

fiducial_slam/test/auto_init_403.test.py

@@ -0,0 +1,83 @@
+import unittest
+
+import launch
+import launch_ros
+import launch_testing
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from ament_index_python.packages import get_package_share_directory
+
+import pytest
+
+@pytest.mark.rostest
+def generate_test_description():
+    test_node = launch_ros.actions.Node(
+        executable=PathJoinSubstitution([LaunchConfiguration('test_binary_dir'), 'auto_init_403_test']),
+        parameters=[
+            {"image_directory":get_package_share_directory('fiducial_slam') + '/test/test_images/'}
+        ],
+        output='screen',
+    )
+
+    static_tranform_publisher = launch_ros.actions.Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='camera_base',
+        arguments="0.035 0.145 0.14 -1.479119 -0.041544 -1.204205 base_link camera".split()
+    )
+
+    aruco_detect = launch_ros.actions.Node(
+        package='aruco_detect',
+        namespace='aruco_detect',
+        executable='aruco_detect',
+        name='aruco_detect',
+        parameters=[
+            {"image_transport":'raw'},
+            {"publish_images":True},
+            {"fiducial_len":0.145},
+        ],
+        #Not sure if the remapping is working properly TODO
+        remappings=[
+            ('camera/compressed', 'camera/image/compressed'),
+            ('fiducial_vertices', '/fiducial_vertices'),
+            ('fiducial_transforms', '/fiducial_transforms')
+        ]
+    )
+
+    fiducial_slam = launch_ros.actions.Node(
+        package='fiducial_slam',
+        executable='fiducial_slam_node',
+        name='fiducial_slam_node',
+        parameters=[
+            {"map_frame":'map'},
+            {"odom_frame":""},
+            {"base_frame":"base_link"},
+            {"future_date_transforms", "0.0"},
+            {"publish_6dof_pose","true"},
+            {"map_file":"/dev/null"}
+        ],
+    )
+
+
+    return launch.LaunchDescription([
+        launch.actions.DeclareLaunchArgument(name='test_binary_dir', # From https://github.com/ros-planning/moveit2/blob/1e1337b46daed0aaa1252b87367c1824f46c9b1a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py#L92
+                                             description='Binary directory of package containing test executables'),
+        # other fixture actions
+        static_tranform_publisher,
+        aruco_detect,
+        fiducial_slam,
+        test_node,
+        #launch_testing.util.KeepAliveProc(), # From https://github.com/ros2/geometry2/blob/a1be44fdbd8c032351ffd3ddff8467d000f4c6d7/test_tf2/test/buffer_client_tester.launch.py#L39
+        launch_testing.actions.ReadyToTest(),
+    ]), locals()
+
+
+class TestTestTermination(unittest.TestCase):
+    def test_termination(self, test_node, proc_output, proc_info):
+        proc_info.assertWaitForShutdown(process=test_node, timeout=(30))
+
+@launch_testing.post_shutdown_test()
+class TestProcessOutput(unittest.TestCase):
+    def test_no_failed_tests(self, proc_output, proc_info, test_node):
+        # Check that process exits with code -15 code: termination request, sent to the program
+        number_of_failed_tests = 0
+        launch_testing.asserts.assertExitCodes(proc_info, [number_of_failed_tests], process=test_node)

fiducial_slam/test/auto_init_403_test.cpp

@@ -6,25 +6,32 @@ and the map entry for the fiducial is reasonable
 
 #include <gtest/gtest.h>
 
-#include <ros/ros.h>
-#include <image_transport/image_transport.h>
-#include <sensor_msgs/CameraInfo.h>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/node.hpp>
+
+#include <image_transport/image_transport.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <cv_bridge/cv_bridge.h>
 
-#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 
-#include <fiducial_msgs/FiducialMapEntry.h>
-#include <fiducial_msgs/FiducialMapEntryArray.h>
+#include "fiducial_msgs/msg/fiducial_map_entry.hpp"
+#include "fiducial_msgs/msg/fiducial_map_entry_array.hpp"
 
 
 class AutoInitTest : public ::testing::Test {
 protected:
-  virtual void SetUp() { 
-    it = new image_transport::ImageTransport(nh);
+  virtual void SetUp() {
+    nh = std::make_shared<rclcpp::Node>("auto_init_403_test");
+
+    // Declare the parameters
+    image_directory = nh->declare_parameter("image_directory", "");
+
+    it = std::make_shared<image_transport::ImageTransport>(nh);
     image_pub = it->advertise("camera/image", 1);
 
-    CameraInfoPub = nh.advertise<sensor_msgs::CameraInfo>("camera_info", 5);
+    camera_info_pub = nh->create_publisher<sensor_msgs::msg::CameraInfo>("/camera/camera_info", 5);
 
     c_info.header.frame_id = "camera";
     c_info.height = 960;
@@ -34,100 +41,101 @@ class AutoInitTest : public ::testing::Test {
     double data_D[] = {0.1349735087283542, -0.2335869827451621,
                        0.0006697030315075139, 0.004846737465872353, 0.0};
 
-    c_info.D = std::vector<double> (data_D,
+    c_info.d = std::vector<double> (data_D,
                                     data_D + sizeof(data_D)/ sizeof(double));
-   
-    c_info.K = {1006.126285753055, 0.0,               655.8639244150409, 
+
+    c_info.k = {1006.126285753055, 0.0,               655.8639244150409, 
                 0.0,               1004.015433012594, 490.6140221242933,
                 0.0,               0.0,               1.0};
 
-    c_info.R = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
+    c_info.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
 
-    c_info.P = {1021.54345703125, 0.0,            661.9091982335958, 0.0,
+    c_info.p = {1021.54345703125, 0.0,            661.9091982335958, 0.0,
                 0.0,              1025.251953125, 490.6380671707448, 0.0,
                 0.0,              0.0,            1.0,               0.0};
 
-    ros::NodeHandle nh_priv("~");
-    nh_priv.getParam("image_directory", image_directory);
+    map_sub = nh->create_subscription<fiducial_msgs::msg::FiducialMapEntryArray>
+        ("/fiducial_map", 1, std::bind(&AutoInitTest::map_callback, this, std::placeholders::_1));
 
-    pose_sub = nh.subscribe("/fiducial_pose", 1, 
-                            &AutoInitTest::pose_callback, this);
-    got_pose = false;
 
-    fiducial_msgs::FiducialMapEntryArray map;
-
-    map_sub = nh.subscribe("/fiducial_map", 1, 
-                            &AutoInitTest::map_callback, this);
-    got_map = false;
+    pose_sub = nh->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>
+        ("/fiducial_pose", 1, std::bind(&AutoInitTest::pose_callback, this, std::placeholders::_1));
   }
 
-  virtual void TearDown() { delete it;}
-
   void publish_image(std::string file)
   {
-    cv::Mat image = cv::imread(image_directory+file, CV_LOAD_IMAGE_COLOR);
-    cv::waitKey(30);
-    sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(),
-                                                   "bgr8", image).toImageMsg();
+    cv::Mat image = cv::imread(image_directory+file, cv::IMREAD_COLOR);
+
+    if(image.data == NULL)
+    {
+      FAIL() << "Cannot open " << image_directory+file;
+    }
+    //cv::waitKey(30); // Should not be needed
+    auto msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", image).toImageMsg();
     image_pub.publish(msg);
-    c_info.header.stamp = rclcpp::Time::now();
-    CameraInfoPub.publish(c_info);
+    c_info.header.stamp = nh->get_clock()->now();
+    camera_info_pub->publish(c_info);
   }
 
-  void map_callback(const fiducial_msgs::FiducialMapEntryArray& msg)
+  void map_callback(const fiducial_msgs::msg::FiducialMapEntryArray::SharedPtr msg)
   {
     got_map = true;
     map = msg;
   }
 
-  void pose_callback(const geometry_msgs::PoseWithCovarianceStamped& msg)
+  void pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
   {
     got_pose = true;
     pose = msg;
   }
 
-  ros::NodeHandle nh;
+  rclcpp::Node::SharedPtr nh;
 
   // Set up Publishing of static images
-  image_transport::ImageTransport* it;
+  std::shared_ptr<image_transport::ImageTransport> it;
   image_transport::Publisher image_pub;
 
-  sensor_msgs::CameraInfo c_info;
-  ros::Publisher CameraInfoPub;
+  sensor_msgs::msg::CameraInfo c_info;
+  rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_pub;
 
   std::string image_directory;
 
-  ros::Subscriber map_sub;
-  ros::Subscriber pose_sub;
+  rclcpp::Subscription<fiducial_msgs::msg::FiducialMapEntryArray>::SharedPtr map_sub;
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub;
 
-  bool got_map;
-  bool got_pose;
+  bool got_map{false};
+  bool got_pose{false};
 
-  fiducial_msgs::FiducialMapEntryArray map;
-  geometry_msgs::PoseWithCovarianceStamped pose;
+  fiducial_msgs::msg::FiducialMapEntryArray::SharedPtr map;
+  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pose;
 
 };
 
 TEST_F(AutoInitTest, tag_403_d7_14cm) {
-  ros::Rate loop_rate(5);
-  while (nh.ok() && (!got_pose || !got_map)) {
+  int loop_count = 0;
+  rclcpp::Rate loop_rate(5);
+  while (rclcpp::ok() && (!got_pose || !got_map)) {
     publish_image("403.jpg");
-    ros::spinOnce();
+    rclcpp::spin_some(nh);
     loop_rate.sleep();
+    loop_count++;
+    if (loop_count > 100) {
+      FAIL() << "Did not receive estimate within 10 frames";
+    }
   }
 
-  ASSERT_NEAR(0, pose.pose.pose.position.x, 0.001);
-  ASSERT_NEAR(0, pose.pose.pose.position.y, 0.001);
-  ASSERT_NEAR(0, pose.pose.pose.position.z, 0.001);
+  ASSERT_NEAR(0, pose->pose.pose.position.x, 0.001);
+  ASSERT_NEAR(0, pose->pose.pose.position.y, 0.001);
+  ASSERT_NEAR(0, pose->pose.pose.position.z, 0.001);
 
-  ASSERT_NEAR(1, pose.pose.pose.orientation.w, 0.001);
-  ASSERT_NEAR(0, pose.pose.pose.orientation.x, 0.001);
-  ASSERT_NEAR(0, pose.pose.pose.orientation.y, 0.001);
-  ASSERT_NEAR(0, pose.pose.pose.orientation.z, 0.001);
+  ASSERT_NEAR(1, pose->pose.pose.orientation.w, 0.001);
+  ASSERT_NEAR(0, pose->pose.pose.orientation.x, 0.001);
+  ASSERT_NEAR(0, pose->pose.pose.orientation.y, 0.001);
+  ASSERT_NEAR(0, pose->pose.pose.orientation.z, 0.001);
 
-  ASSERT_LE(1, map.fiducials.size());
-  
-  const fiducial_msgs::FiducialMapEntry &fid = map.fiducials[0];
+  ASSERT_LE(1, map->fiducials.size());
+
+  const fiducial_msgs::msg::FiducialMapEntry &fid = map->fiducials[0];
   ASSERT_EQ(403, fid.fiducial_id);
   ASSERT_NEAR(0.7611, fid.x, 0.001);
   ASSERT_NEAR(0.2505, fid.y, 0.001);
@@ -139,8 +147,7 @@ TEST_F(AutoInitTest, tag_403_d7_14cm) {
 
 int main(int argc, char** argv)
 {
-
   testing::InitGoogleTest(&argc, argv);
-  ros::init(argc, argv, "AutoInitTest");
+  rclcpp::init(argc, argv);
   return RUN_ALL_TESTS();
 }