From 03d52dd5a70582a8169ee33f2a626a349a0384c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 22 Jan 2024 16:13:43 +0100 Subject: [PATCH] Port image_proc test to ROS 2 (#910) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- image_proc/CMakeLists.txt | 8 ++ image_proc/package.xml | 1 + image_proc/test/CMakeLists.txt | 5 - .../test/resources/calibration_file.ini | 29 +++++ image_proc/test/resources/logo.png | Bin 0 -> 7133 bytes image_proc/test/rostest.cpp | 93 ++++++++-------- image_proc/test/test_bayer.xml | 10 -- image_proc/test/test_rectify.cpp | 99 +++++++++++------- image_proc/test/test_rectify.xml | 9 -- 9 files changed, 147 insertions(+), 107 deletions(-) delete mode 100644 image_proc/test/CMakeLists.txt create mode 100644 image_proc/test/resources/calibration_file.ini create mode 100644 image_proc/test/resources/logo.png delete mode 100644 image_proc/test/test_bayer.xml delete mode 100644 image_proc/test/test_rectify.xml 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 @@ ament_cmake_auto + camera_calibration_parsers cv_bridge image_geometry image_transport 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 0000000000000000000000000000000000000000..6c38d3e3a2957d90fb4db812a7347ed9d5dd996d GIT binary patch literal 7133 zcma)>Wm6msthN^~?hcDB?i49rthg=iP>L6~#oeJ;@uFLxi%Ti)UbMKoE$%MoIq&-i z&X;R4nR_Oge7cjwXsF3!V^ClK003-71z9ivfPnj7-0}|TKSqUo2>dTWt)ZeV_xASo z`ua-Gs|EmY9iNyaSMl0swdbA`t+92cKTL zpt#z?%6(*F9sr>5i^%xu=n4Sv1F!|9WOb_Q+vAdcWMo4fq1G>4g{+BHaTM5z#5+6nv(p)&Sa11gt7RdUhdkMX;%d znvR3{XD3ohZdd;VB3dyp*i1@VlZ8!`fRI>BR7y}#NK)B4G%S*pg;7IYou6NZn1uFQ zQd&<>Hx1K!nNR9i#7rO-zW0C=s{d_NQC3PD^5et=({HJ1D!m&dSj(FMQlrWSbT|Dr z=LrulFN8;cZCf3E1(#QUzrES`GYorUeGq@Sb(_07uJ5m_sha?tX&QIeX+QC>f`~8Rox_Bd#M2k&486)dQuHd!4 z)!kQ$&pp1BYWy~a&YXAqSr5-Hi#dNq9cZKDvJE9GHl?3N(nYLjmM?ERBAHX+IMt}c z`%wA6knYI5#L$PE6&ZgH- z&PEg`1|D@}Yzunq!b1mMry4(#ZjH8C6o$W^wj~Mdv&HsYXD;lXUn?l>T>0eTJ!^~_ zTd13zeVgV}Is0clNSlXMJtuq?Jq_I)+{di-SoK@>MaFtGlZxZGpe%`_hWUINe&d`6;?! zfLS_5%(*l&A>(ok3t#Zplb_ieggQ>NtybpgsHC8WBHimkS@&u+zGGiZGThQDu4d|O zOm@fCJ|cM=a9(=Bn;y{w(!@T+v?u;2Lm#U`3sP`fV%sGjJ&@;AGvqpRf^=m#jAT78 z#cB7!UB>Ge0HU$3Tv^O+n+r$7?N|2dED*V}v{4=;tJJL-Owt%lnK={8S?w@@vFW-6 zdQRbo(O0;pj8Q2yNMlH`!n9Ltr`y0Wv1m{m;_~`u(baS^)Z!0l=XHBbM`*FDtFaWJ zRS9Upv<5#FtWiX)g~09kPRA6`gsAVi!MyCMmL(Zt zrsYthBFe(fpM>0*Uvvgb1wY)3NdE1F_t3G{^RpuB#yT(z!@MeeiF8bpG}4U4vgObc znYn=AxlqM$^zX)EEe`Q#TgmucQ!Y+f8(;fxLw@5c@L?cl$M+;x6c2&7{o;0XgRI;q zW*ou5DG9}wKW*03NjfzPF#+bKwdNW3qxs||L`o0 zvk*M-CSoFNa4ER2m=UL>wo1x;m0MrmM!uhCJ2F!?K$$z|J@a6JY>Lv+;iw#_t{I2@ z)41--gLjz;G}>dF*SYN|&R%510!j^D78FZT(Rs(c+mKXr1u_ARs`x@Rxf(f&%dTM< zAwN5$7oaVw^z=xG)1SYOZWw@W5`42!@s72X|eo~JN2{MMybOZy;1o`{LZatRDVRb<|CBtLCt7XWu zpDM`f?41T>WS7XTT1JxSaM?VzQV-J?krUQ_vdO|zMS?AzFNyMR;15zMHv7g8MaFlI zu5zIH39i4S`<(guJrlZ{TU%Sdfx6RNuOIidp@K_wL8C~BUInX9M_)Bx z^d27TH@aru!T2$b?^kykUMeT?Lx*ois3H8(4?TL;4rP!|sWp}a&+>U?9@R4#0eI+g zpx|+0px1h$KwbvR+vBTC++?2#goTOx#${&^sqZ}ObFIAZeA8h%w(@mzn7rMe_8I6q ztQMZPaa8$*$Q2eupjd6i{5f8-??*HPyR%>wgb&cgGQsY=LoxVUCU(? zDx5%!Jk>kYTK#Ji79UC2S}T_|huPY7ha)4>n8X6xQD9^#foON`=kBr>Ndx#K==$J; zZ@Pcghrt0~&YH){{;3V?G6#_n*3qa{%i*0P85yhpagS>C5Zp$fHp|q$F?m#VyLZe? z7r_>|3MGb@Y8UpXMZH^&*cKCDH?b_S{}Z%@7Qt-AF@VWHe6F-K+|e%drEu6+e?n+0 z#E&WQpupm&7-a^{2r6g;=RpM0D`|D&uabnOk1um!0!0hi%lCS0;c~5=iHu%T+&abf zyOyi_X|f}U#3^D|$mX<2VzDPlGkW^7Mpzls%BA2vUgHEdbGB%jOI z!`f+2?D!0sX@IT6^&Q65#lbdNg@-~#x;SUo z7`HWV?QHR;7Mr584cQmF;c@G~~C<6}!;1B2&S+s`HZzhYBK4pMywhL@2WPK%DLlU3F8qWPX)1w!g^ny7&aHx8y^k<9d6+BdP+nDvvUR}1`eZ;ZG zGa8wwduS1hFLr?TV?S)|^k-~0LLE%aC?6HeIdZ4NbV600gkoZVno!H+FWu~(Zw&qEp2ko{I|>i`;cZU?v>w%X4RM#US!k|bNy7m3~q|M?3MZw@mOz* z28=F+3Oc}PX$hxeCQj_7pUvTgw)n!DRW|6ry){cU$2X#-U9 zn%n?`e?RR$NpD4b!uT^aB-qEJ+u0oNWkAB{j}8XeOnv&^yRwWVmd3h2uhB`=nxbLY zqGbxHLc2JXc4)u@CtKcVGme5<>sbjvU4K?YqsYu}J)EKnuBK=esQCj_MtBLRr{c{7 z9i9mqzz;0dO4V#l#VPte#{|`7qppbX@@{PlMiquaeWuWXcvE(n*j{Nlm1TDj~Xz)W!;Urop+*^xE zAB)dy8I8Y^anG&KDHfln$Z>})U{%Wtlb${s?8l)Q{bxJlrUP~^R-K8RA39afp6y@Y zcPiZTr!h2@kg<*1$PDbg(OqB{V&7|Qf0ELJ;u*>^Y;a$N4Z`$!ti$$xMt=nmxNd!ziGrpj~NOnLDczfSM=+La_Ic3SD!<68fAPz)?8>K;~c$) z%|9Z1`lJHvvoIXnd^aGzH`9W`E7X6%NlRRl^t~@EheS_<#)Iao{m{U}KFaasBEld$ zJpw@iLkEmNAs|+2o#4hk!YzuF^AryyfR7Dzj|x|BZqVxpB_~bpyRY7+4K;p*3wmJQ z!eD}XuH)xj4d0DN_3~RE$NPNvqxx7IwB`AAx&i8pe~(l?M$YtvZ?Wm(&;Sv!mWu=R@Z?joUpxr`z-Fa8BpQb7*VS-z6m}cp2(B;AZ#)Ieq z>zzkIYVmEwrN>rD+%NBoCO;;`oF8#(J*EX+=GvAkIx&W^ABGVe)1%Z~Sz z_$vbp^8h*RquqVa}RIP@phqP4K{CXRfU zWOCpJ_kJ_u>tIbZYf0wk*#Gbv#c~}N_a<@)ps(lk`D1qcrQ@xdc@WlHz?#vn^4bb> zotvZ*3wI4XTUTq_Y-#>NM?9S$=y6CR`}&c$nUo@sjqS5z_svmldyGcl2ax7qYp|;UHVZ@hcpJ6~MGZL5=J1OX7>Ub7kvGwUU z49~0aFYLfZQ{)Ohy+46_^&1z@cH>zKGEI1f=1yn!vU%gImw%Y3t${U_zEkGn>e6}? zCj}VXEH(REE!ny5vW_zNagLk$g{=7Q^r-T2tzU5|mLAUp@lE{9x04361V65B<7nNt z+(F4uDAq#y_6>utTX~RXJo*nm?=>>6Wx_Q+B#r`uErJ+!Bzb^R#f8$ChM zo`Y?=9<*oJKVAMbZ==~6gaH9ee%@~s#goa+;Z^wIdY?MMU9W2XeH^X_uo>?D%s>hG z-xHcrqU{eFqmy?pHgZ2b)%VZ+4Dvpv4G8N7DXT>&j_luF6oMoB9B$l=6UXt1M=6?K z46NQA>!_@y%%cXINCp%xLu{SLnFi(3sYFx)^e)W4Omg`uK)U2KG0urPG<{&X8(O-R zgtlgsKWL=I{RJh%?wm>Rar=JeoQYeT9?IFq=DthfXqvT$2sp-hPcalxnwDEv46C-0-cK{Shq+nC7feAiT1f+(T-1 zta2w`x%6Z$>ic;XdFL#V2jV5ITZulH!p{?#EQQEvs0T(-yJDszYB~jlGa{VVXp4Os z|NYIuLJaw5Kvi6U`p$4Et39bU_VlaG!NXxTo89f*dUbU*sVc%Y-G)xwR)`TY%0Hi6 zX}D1sv6TCv@!)GHzOE6&cA<(B|23ES7dei|{-r?&&ICPXeiX3+y~=4EyZReIfY0>6 zQ9*G3H>vsNXqp3cA?H#6S%F8zkw+Hwp144DgE&q85GZ{5mf%73Xycf<}T&nFuT;wYXe1 zx`0SplJ^``wQVQ}NId(7Rma4CAWs{V&8^Qps;0RZU4{^8KgPH(biVQcC(doqclJ=? z7V4xpfpP-NrSVeD|sWD3#HYk&TrP6?}D8s1d<-U7L z9NkD5yXrCD6MnV*6u;cs>JGF^$svg%Z5jL$Dfs0S^{T~N_k9{#WfuulQFEtv64%Htx1tiN#euClrCK*x2myYfyK=~R zWU38wPl2R6#rKbev-V-&zPP#Q`tx8rYlonZnB|MG`Zb^gnYfAbdOBPuiU5aZ>Aw{Bjpfd&|I-+Odb?9Yy$7EP5vx0xkv zuY%f&q~QaLcUK{wenPJ6x);4uH(W2Dca|3!U(0yT`*h?*TYbw^VlGuqaytS8O#5#K zt(K=sSB@lGas?AFeyoX4Fa}H3cv=s~nRYo~ESE`CV<0XVkN`sw9Q{Q#wbPf{eZKkI znLv#0Z(oem2E|oY#U;mxk}J9arlblvFM|EO$r%cuIu4!aE#B7yDnEaJm1-D?RDKuI zVtnt{B{-HuqQ@~Y`EwZyp-RI9+qN|*jd~lZ?Dmh1Hns6OImGvy>l6Je9#F1Gxm3g!{;I5$20ULLD*a7X-xrNQDnR07AgHn`E=MI{`NyBCjqj6x^x%KPT;Nv z=*Y1~V={3=w=*|rgk7k#vjlx#v;=-cygl+Dv$G!cXl$2<7Sz&-cto9^A-KJ~w^S?^ zMa=0PYLpnnjZp!~fx~a=y9&r1pks$OW+@IgeoHRPSVu?sissyA4S_v(^33~GgN)MZ zKjMB} + +#include #include -#include "ros/ros.h>" -#include "gtest/gtest.h" -#include "cv_bridge/cv_bridge.hpp" +#include +#include #include #include #include -#include +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( + 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 @@ - - - - - - - - - - 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 + #include +#include #include +#include -#include "ros/ros.h" -#include "gtest/gtest.h" -#include "cv_bridge/cv_bridge.hpp" +#include +#include #include #include #include #include -#include +#include + +#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(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(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 @@ - - - - - - - -