From 91da2263c3b5ce493aca87bd91104b8e2790818f Mon Sep 17 00:00:00 2001 From: Alejandro Hernandez Cordero Date: Tue, 27 May 2025 18:49:21 +0200 Subject: [PATCH 1/5] Added camera info manager unit test Signed-off-by: Alejandro Hernandez Cordero --- camera_info_manager/CMakeLists.txt | 16 +- .../camera_info_manager.hpp | 6 +- camera_info_manager/tests/unit_test.cpp | 477 +++++++++--------- camera_info_manager/tests/unit_test.test | 17 - 4 files changed, 256 insertions(+), 260 deletions(-) delete mode 100644 camera_info_manager/tests/unit_test.test diff --git a/camera_info_manager/CMakeLists.txt b/camera_info_manager/CMakeLists.txt index e26ef882..3314c851 100644 --- a/camera_info_manager/CMakeLists.txt +++ b/camera_info_manager/CMakeLists.txt @@ -58,10 +58,18 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - #TODO(mjcarroll) Enable unit test with rostest - #add_executable(unit_test tests/unit_test.cpp) - #target_link_libraries(unit_test ${PROJECT_NAME}) - #add_rostest(tests/unit_test.test DEPENDENCIES unit_test) + ament_add_gtest(${PROJECT_NAME}-unit_test + tests/unit_test.cpp + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" + ) + target_link_libraries(${PROJECT_NAME}-unit_test + ${PROJECT_NAME} + ${sensor_msgs_TARGETS} + rclcpp::rclcpp + ) + + # copy the file in the installation folder to test with package:// + install(FILES tests/test_calibration.yaml DESTINATION share/${PROJECT_NAME}/tests) endif() ament_package() diff --git a/camera_info_manager/include/camera_info_manager/camera_info_manager.hpp b/camera_info_manager/include/camera_info_manager/camera_info_manager.hpp index 590bd9b8..e1aa0439 100644 --- a/camera_info_manager/include/camera_info_manager/camera_info_manager.hpp +++ b/camera_info_manager/include/camera_info_manager/camera_info_manager.hpp @@ -186,14 +186,14 @@ class CameraInfoManager rclcpp::Node * node, const std::string & cname = "camera", const std::string & url = "", - const std::string & ns = "~"); + const std::string & ns = ""); CAMERA_INFO_MANAGER_PUBLIC CameraInfoManager( rclcpp_lifecycle::LifecycleNode * node, const std::string & cname = "camera", const std::string & url = "", - const std::string & ns = "~"); + const std::string & ns = ""); CAMERA_INFO_MANAGER_PUBLIC CameraInfoManager( @@ -202,7 +202,7 @@ class CameraInfoManager rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logger_interface, const std::string & cname = "camera", const std::string & url = "", rmw_qos_profile_t custom_qos = rmw_qos_profile_default, - const std::string & ns = "~"); + const std::string & ns = ""); CAMERA_INFO_MANAGER_PUBLIC CameraInfo getCameraInfo(void); diff --git a/camera_info_manager/tests/unit_test.cpp b/camera_info_manager/tests/unit_test.cpp index 9af99729..06ac0856 100644 --- a/camera_info_manager/tests/unit_test.cpp +++ b/camera_info_manager/tests/unit_test.cpp @@ -31,26 +31,41 @@ #include +#include +#include +#include +#include #include #include #include "camera_info_manager/camera_info_manager.hpp" +#include "rcpputils/env.hpp" +#include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/distortion_models.hpp" +#include "rclcpp/rclcpp.hpp" + /////////////////////////////////////////////////////////////// // global test data /////////////////////////////////////////////////////////////// -namespace +class CameraInfoManagerTesting : public ::testing::Test { -const char g_package_name[] = "camera_info_manager"; -const char g_test_name[] = "test_calibration"; -const char g_package_filename[] = "/tests/" + g_test_name + ".yaml"; -const char g_package_url[] = "package://" + g_package_name + g_package_filename; -const char g_package_name_url = "package://" + g_package_name + "/tests/${NAME}.yaml"; -const char g_default_url = "file://${ROS_HOME}/camera_info/${NAME}.yaml"; -const char g_camera_name = "08144361026320a0"; -} // namespace +protected: + void SetUp() + { + node = rclcpp::Node::make_shared("camera_info_manager_test"); + } + + rclcpp::Node::SharedPtr node; + std::string g_package_name = "camera_info_manager"; + std::string g_test_name = "test_calibration"; + std::string g_package_filename = "/tests/" + g_test_name + ".yaml"; + std::string g_package_url = "package://" + g_package_name + g_package_filename; + std::string g_package_name_url = "package://" + g_package_name + "/tests/${NAME}.yaml"; + std::string g_default_url = "file://${ROS_HOME}/camera_info/${NAME}.yaml"; + std::string g_camera_name = "08144361026320a0"; +}; /////////////////////////////////////////////////////////////// // utility functions @@ -58,8 +73,8 @@ const char g_camera_name = "08144361026320a0"; // compare CameraInfo fields that are saved and loaded for calibration void compare_calibration( - const sensor_msgs::CameraInfo & exp, - const sensor_msgs::CameraInfo & ci) + const sensor_msgs::msg::CameraInfo & exp, + const sensor_msgs::msg::CameraInfo & ci) { // check image size EXPECT_EQ(exp.width, ci.width); @@ -67,142 +82,151 @@ void compare_calibration( // check distortion coefficients EXPECT_EQ(exp.distortion_model, ci.distortion_model); - EXPECT_EQ(exp.D.size(), ci.D.size()); - for (unsigned i = 0; i < ci.D.size(); ++i) { - EXPECT_EQ(exp.D[i], ci.D[i]); + EXPECT_EQ(exp.d.size(), ci.d.size()); + for (unsigned i = 0; i < ci.d.size(); ++i) { + EXPECT_EQ(exp.d[i], ci.d[i]); } // check camera matrix - for (unsigned i = 0; i < ci.K.size(); ++i) { - EXPECT_EQ(exp.K[i], ci.K[i]); + for (unsigned i = 0; i < ci.k.size(); ++i) { + EXPECT_EQ(exp.k[i], ci.k[i]); } // check rectification matrix - for (unsigned i = 0; i < ci.R.size(); ++i) { - EXPECT_EQ(exp.R[i], ci.R[i]); + for (unsigned i = 0; i < ci.r.size(); ++i) { + EXPECT_EQ(exp.r[i], ci.r[i]); } // check projection matrix - for (unsigned i = 0; i < ci.P.size(); ++i) { - EXPECT_EQ(exp.P[i], ci.P[i]); + for (unsigned i = 0; i < ci.p.size(); ++i) { + EXPECT_EQ(exp.p[i], ci.p[i]); } } // make sure this file does not exist void delete_file(std::string filename) { - nnt rc = unlink(filename.c_str()); - if (rc != 0) { - if (errno != ENOENT) { - ROS_INFO_STREAM("unexpected unlink() error: " << errno); - } - } + std::filesystem::remove(filename); } void delete_default_file(void) { std::string ros_home("/tmp"); - setenv("ROS_HOME", ros_home.c_str(), true); std::string tmpFile(ros_home + "/camera_info/camera.yaml"); - delete_file(tmpFile); + std::filesystem::remove(tmpFile); } -void do_system(const std::string & command) -{ - int rc = system(command.c_str()); - if (rc) { - std::cout << command << " returns " << rc; - } -} +// void do_system(const std::string & command) +// { +// int rc = system(command.c_str()); +// if (rc) { +// std::cout << command << " returns " << rc; +// } +// } void delete_tmp_camera_info_directory(void) { - do_system(std::string("rm -rf /tmp/camera_info")); + std::filesystem::remove("/tmp/camera_info"); } -void make_tmp_camera_info_directory(void) -{ - do_system(std::string("mkdir -p /tmp/camera_info")); -} +// void make_tmp_camera_info_directory(void) +// { +// do_system(std::string("mkdir -p /tmp/camera_info")); +// } // These data must match the contents of test_calibration.yaml. -sensor_msgs::CameraInfo expected_calibration(void) +sensor_msgs::msg::CameraInfo expected_calibration(void) { - sensor_msgs::CameraInfo ci; + sensor_msgs::msg::CameraInfo ci; ci.width = 640u; ci.height = 480u; // set distortion coefficients ci.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; - ci.D.resize(5); - ci.D[0] = -1.04482; - ci.D[1] = 1.59252; - ci.D[2] = -0.0196308; - ci.D[3] = 0.0287906; - ci.D[4] = 0.0; + ci.d.resize(5); + ci.d[0] = -1.04482; + ci.d[1] = 1.59252; + ci.d[2] = -0.0196308; + ci.d[3] = 0.0287906; + ci.d[4] = 0.0; // set camera matrix - ci.K[0] = 1168.68; - ci.K[1] = 0.0; - ci.K[2] = 295.015; - ci.K[3] = 0.0; - ci.K[4] = 1169.01; - ci.K[5] = 252.247; - ci.K[6] = 0.0; - ci.K[7] = 0.0; - ci.K[8] = 1.0; + ci.k[0] = 1168.68; + ci.k[1] = 0.0; + ci.k[2] = 295.015; + ci.k[3] = 0.0; + ci.k[4] = 1169.01; + ci.k[5] = 252.247; + ci.k[6] = 0.0; + ci.k[7] = 0.0; + ci.k[8] = 1.0; // set rectification matrix - ci.R[0] = 1.0; - ci.R[1] = 0.0; - ci.R[2] = 0.0; - ci.R[3] = 0.0; - ci.R[4] = 1.0; - ci.R[5] = 0.0; - ci.R[6] = 0.0; - ci.R[7] = 0.0; - ci.R[8] = 1.0; + ci.r[0] = 1.0; + ci.r[1] = 0.0; + ci.r[2] = 0.0; + ci.r[3] = 0.0; + ci.r[4] = 1.0; + ci.r[5] = 0.0; + ci.r[6] = 0.0; + ci.r[7] = 0.0; + ci.r[8] = 1.0; // set projection matrix - ci.P[0] = ci.K[0]; - ci.P[1] = ci.K[1]; - ci.P[2] = ci.K[2]; - ci.P[3] = 0.0; - ci.P[4] = ci.K[3]; - ci.P[5] = ci.K[4]; - ci.P[6] = ci.K[5]; - ci.P[7] = 0.0; - ci.P[8] = ci.K[6]; - ci.P[9] = ci.K[7]; - ci.P[10] = ci.K[8]; - ci.P[11] = 0.0; + ci.p[0] = ci.k[0]; + ci.p[1] = ci.k[1]; + ci.p[2] = ci.k[2]; + ci.p[3] = 0.0; + ci.p[4] = ci.k[3]; + ci.p[5] = ci.k[4]; + ci.p[6] = ci.k[5]; + ci.p[7] = 0.0; + ci.p[8] = ci.k[6]; + ci.p[9] = ci.k[7]; + ci.p[10] = ci.k[8]; + ci.p[11] = 0.0; return ci; } // issue SetCameraInfo service request bool set_calibration( - rclcpp::Node * node, - const sensor_msgs::CameraInfo & calib) -{ - ros::ServiceClient client = - node.serviceClient("set_camera_info"); - sensor_msgs::SetCameraInfo set_camera_info; - set_camera_info.request.camera_info = calib; - bool success; - EXPECT_TRUE((success = client.call(set_camera_info))); - return success; + std::shared_ptr node, + const sensor_msgs::msg::CameraInfo & calib) +{ + auto client = node->create_client("/set_camera_info"); + while (!client->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "client interrupted while waiting for service to appear."); + return 1; + } + RCLCPP_INFO(node->get_logger(), "waiting for service to appear..."); + } + + auto request = std::make_shared(); + request->camera_info = calib; + auto result_future = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(node, result_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node->get_logger(), "service call failed :("); + client->remove_pending_request(result_future); + return false; + } + result_future.get(); + + return true; } // resolve URL string, result should be as expected void check_url_substitution( - ros::NodeHandle node, + std::shared_ptr node, const std::string & url, const std::string & exp_url, const std::string & camera_name) { - camera_info_manager::CameraInfoManager cinfo(node, camera_name, url); + camera_info_manager::CameraInfoManager cinfo(node.get(), camera_name, url); std::string sub_url = cinfo.resolveURL(url, camera_name); EXPECT_EQ(sub_url, exp_url); } @@ -212,10 +236,9 @@ void check_url_substitution( /////////////////////////////////////////////////////////////// // Test that valid camera names are accepted -TEST(CameraName, validNames) +TEST_F(CameraInfoManagerTesting, validNames) { - ros::NodeHandle node; - camera_info_manager::CameraInfoManager cinfo(node); + camera_info_manager::CameraInfoManager cinfo(node.get()); EXPECT_TRUE(cinfo.setCameraName(std::string("a"))); EXPECT_TRUE(cinfo.setCameraName(std::string("1"))); @@ -230,10 +253,9 @@ TEST(CameraName, validNames) } // Test that invalid camera names are rejected -TEST(CameraName, invalidNames) +TEST_F(CameraInfoManagerTesting, invalidNames) { - ros::NodeHandle node; - camera_info_manager::CameraInfoManager cinfo(node); + camera_info_manager::CameraInfoManager cinfo(node.get()); EXPECT_FALSE(cinfo.setCameraName(std::string(""))); EXPECT_FALSE(cinfo.setCameraName(std::string("-21"))); @@ -243,10 +265,9 @@ TEST(CameraName, invalidNames) } // Test that valid URLs are accepted -TEST(UrlValidation, validURLs) +TEST_F(CameraInfoManagerTesting, validURLs) { - ros::NodeHandle node; - camera_info_manager::CameraInfoManager cinfo(node); + camera_info_manager::CameraInfoManager cinfo(node.get()); EXPECT_TRUE(cinfo.validateURL(std::string(""))); EXPECT_TRUE(cinfo.validateURL(std::string("file:///"))); @@ -260,10 +281,9 @@ TEST(UrlValidation, validURLs) } // Test that invalid URLs are rejected -TEST(UrlValidation, invalidURLs) +TEST_F(CameraInfoManagerTesting, invalidURLs) { - ros::NodeHandle node; - camera_info_manager::CameraInfoManager cinfo(node); + camera_info_manager::CameraInfoManager cinfo(node.get()); EXPECT_FALSE(cinfo.validateURL(std::string("file://"))); EXPECT_FALSE(cinfo.validateURL(std::string("flash:///"))); @@ -275,72 +295,67 @@ TEST(UrlValidation, invalidURLs) } // Test ability to provide uncalibrated CameraInfo -TEST(GetInfo, uncalibrated) +TEST_F(CameraInfoManagerTesting, uncalibrated) { - ros::NodeHandle node; - delete_default_file(); - camera_info_manager::CameraInfoManager cinfo(node); + camera_info_manager::CameraInfoManager cinfo(node.get()); EXPECT_FALSE(cinfo.isCalibrated()); - sensor_msgs::CameraInfo ci(cinfo.getCameraInfo()); - sensor_msgs::CameraInfo exp; + sensor_msgs::msg::CameraInfo ci(cinfo.getCameraInfo()); + sensor_msgs::msg::CameraInfo exp; compare_calibration(exp, ci); } // Test ability to load calibrated CameraInfo -TEST(GetInfo, calibrated) +TEST_F(CameraInfoManagerTesting, calibrated) { - ros::NodeHandle node; - delete_default_file(); - camera_info_manager::CameraInfoManager cinfo(node); + camera_info_manager::CameraInfoManager cinfo(node.get()); EXPECT_FALSE(cinfo.isCalibrated()); - std::string pkgPath(ros::package::getPath(g_package_name)); - std::string url("file://" + pkgPath + "/tests/test_calibration.yaml"); + std::string current_path = std::filesystem::current_path(); + std::string url("file://" + current_path + "/tests/test_calibration.yaml"); EXPECT_TRUE(cinfo.loadCameraInfo(url)); EXPECT_TRUE(cinfo.isCalibrated()); - sensor_msgs::CameraInfo ci(cinfo.getCameraInfo()); - sensor_msgs::CameraInfo exp(expected_calibration()); + sensor_msgs::msg::CameraInfo ci(cinfo.getCameraInfo()); + sensor_msgs::msg::CameraInfo exp(expected_calibration()); compare_calibration(exp, ci); } // Test ability to load calibrated CameraInfo from package -TEST(GetInfo, fromPackage) +TEST_F(CameraInfoManagerTesting, fromPackage) { - ros::NodeHandle node; - camera_info_manager::CameraInfoManager cinfo(node); + camera_info_manager::CameraInfoManager cinfo(node.get()); + + std::cout << "g_package_url " << g_package_url << std::endl; EXPECT_TRUE(cinfo.loadCameraInfo(g_package_url)); EXPECT_TRUE(cinfo.isCalibrated()); - sensor_msgs::CameraInfo ci(cinfo.getCameraInfo()); - sensor_msgs::CameraInfo exp(expected_calibration()); + sensor_msgs::msg::CameraInfo ci(cinfo.getCameraInfo()); + sensor_msgs::msg::CameraInfo exp(expected_calibration()); compare_calibration(exp, ci); } // Test ability to access named calibrated CameraInfo from package -TEST(GetInfo, fromPackageWithName) +TEST_F(CameraInfoManagerTesting, fromPackageWithName) { - ros::NodeHandle node; - camera_info_manager::CameraInfoManager cinfo(node, g_test_name, + camera_info_manager::CameraInfoManager cinfo(node.get(), g_test_name, g_package_name_url); EXPECT_TRUE(cinfo.isCalibrated()); - sensor_msgs::CameraInfo ci(cinfo.getCameraInfo()); - sensor_msgs::CameraInfo exp(expected_calibration()); + sensor_msgs::msg::CameraInfo ci(cinfo.getCameraInfo()); + sensor_msgs::msg::CameraInfo exp(expected_calibration()); compare_calibration(exp, ci); } // Test load of unresolved "package:" URL files -TEST(GetInfo, unresolvedLoads) +TEST_F(CameraInfoManagerTesting, unresolvedLoads) { - ros::NodeHandle node; - camera_info_manager::CameraInfoManager cinfo(node); + camera_info_manager::CameraInfoManager cinfo(node.get()); EXPECT_FALSE(cinfo.loadCameraInfo(std::string("package://"))); EXPECT_FALSE(cinfo.isCalibrated()); @@ -348,7 +363,8 @@ TEST(GetInfo, unresolvedLoads) EXPECT_FALSE(cinfo.loadCameraInfo(std::string("package:///calibration.yaml"))); EXPECT_FALSE(cinfo.isCalibrated()); - EXPECT_FALSE(cinfo.loadCameraInfo(std::string("package://no_such_package/calibration.yaml"))); + EXPECT_THROW(cinfo.loadCameraInfo(std::string("package://no_such_package/calibration.yaml")), + std::logic_error); EXPECT_FALSE(cinfo.isCalibrated()); EXPECT_FALSE( @@ -358,29 +374,27 @@ TEST(GetInfo, unresolvedLoads) } // Test load of "package:" URL after changing name -TEST(GetInfo, nameChange) +TEST_F(CameraInfoManagerTesting, nameChange) { - ros::NodeHandle node; const std::string missing_file("no_such_file"); // first declare using non-existent camera name - camera_info_manager::CameraInfoManager cinfo(node, missing_file, + camera_info_manager::CameraInfoManager cinfo(node.get(), missing_file, g_package_name_url); EXPECT_FALSE(cinfo.isCalibrated()); // set name so it resolves to a test file that does exist EXPECT_TRUE(cinfo.setCameraName(g_test_name)); EXPECT_TRUE(cinfo.isCalibrated()); - sensor_msgs::CameraInfo ci(cinfo.getCameraInfo()); - sensor_msgs::CameraInfo exp(expected_calibration()); + sensor_msgs::msg::CameraInfo ci(cinfo.getCameraInfo()); + sensor_msgs::msg::CameraInfo exp(expected_calibration()); compare_calibration(exp, ci); } // Test load of invalid CameraInfo URLs -TEST(GetInfo, invalidLoads) +TEST_F(CameraInfoManagerTesting, invalidLoads) { - ros::NodeHandle node; - camera_info_manager::CameraInfoManager cinfo(node); + camera_info_manager::CameraInfoManager cinfo(node.get()); EXPECT_FALSE(cinfo.loadCameraInfo(std::string("flash:///"))); EXPECT_FALSE(cinfo.isCalibrated()); @@ -388,19 +402,18 @@ TEST(GetInfo, invalidLoads) EXPECT_FALSE(cinfo.loadCameraInfo(std::string("html://ros.org/wiki/camera_info_manager"))); EXPECT_FALSE(cinfo.isCalibrated()); - sensor_msgs::CameraInfo ci(cinfo.getCameraInfo()); - sensor_msgs::CameraInfo exp; + sensor_msgs::msg::CameraInfo ci(cinfo.getCameraInfo()); + sensor_msgs::msg::CameraInfo exp; compare_calibration(exp, ci); } // Test ability to set CameraInfo directly -TEST(SetInfo, setCameraInfo) +TEST_F(CameraInfoManagerTesting, setCameraInfo) { - ros::NodeHandle node; - camera_info_manager::CameraInfoManager cinfo(node); + camera_info_manager::CameraInfoManager cinfo(node.get()); // issue calibration service request - sensor_msgs::CameraInfo exp(expected_calibration()); + sensor_msgs::msg::CameraInfo exp(expected_calibration()); bool success = cinfo.setCameraInfo(exp); EXPECT_TRUE(success); @@ -409,19 +422,18 @@ TEST(SetInfo, setCameraInfo) if (success) { // check that it worked EXPECT_TRUE(cinfo.isCalibrated()); - sensor_msgs::CameraInfo ci = cinfo.getCameraInfo(); + sensor_msgs::msg::CameraInfo ci = cinfo.getCameraInfo(); compare_calibration(exp, ci); } } // Test ability to set calibrated CameraInfo -TEST(SetInfo, setCalibration) +TEST_F(CameraInfoManagerTesting, setCalibration) { - ros::NodeHandle node; - camera_info_manager::CameraInfoManager cinfo(node); + camera_info_manager::CameraInfoManager cinfo(node.get()); // issue calibration service request - sensor_msgs::CameraInfo exp(expected_calibration()); + sensor_msgs::msg::CameraInfo exp(expected_calibration()); bool success = set_calibration(node, exp); // only check results if the service succeeded, avoiding confusing @@ -429,26 +441,29 @@ TEST(SetInfo, setCalibration) if (success) { // check that it worked EXPECT_TRUE(cinfo.isCalibrated()); - sensor_msgs::CameraInfo ci = cinfo.getCameraInfo(); + sensor_msgs::msg::CameraInfo ci = cinfo.getCameraInfo(); compare_calibration(exp, ci); } + + std::string home; + home = rcpputils::get_env_var("HOME"); + delete_file(home + "/.ros/camera_info/camera.yaml"); } // Test ability to save calibrated CameraInfo in default URL -TEST(SetInfo, saveCalibrationDefault) +TEST_F(CameraInfoManagerTesting, saveCalibrationDefault) { - ros::NodeHandle node; - sensor_msgs::CameraInfo exp(expected_calibration()); + sensor_msgs::msg::CameraInfo exp(expected_calibration()); bool success; // Set ${ROS_HOME} to /tmp, then delete the /tmp/camera_info // directory and everything in it. - setenv("ROS_HOME", "/tmp", true); + rcpputils::set_env_var("ROS_HOME", "/tmp"); delete_tmp_camera_info_directory(); { // create instance to save calibrated data - camera_info_manager::CameraInfoManager cinfo(node); + camera_info_manager::CameraInfoManager cinfo(node.get()); EXPECT_FALSE(cinfo.isCalibrated()); // issue calibration service request @@ -460,10 +475,10 @@ TEST(SetInfo, saveCalibrationDefault) // and redundant failure messages if (success) { // create a new instance to load saved calibration - camera_info_manager::CameraInfoManager cinfo2(node); + camera_info_manager::CameraInfoManager cinfo2(node.get()); EXPECT_TRUE(cinfo2.isCalibrated()); if (cinfo2.isCalibrated()) { - sensor_msgs::CameraInfo ci(cinfo2.getCameraInfo()); + sensor_msgs::msg::CameraInfo ci(cinfo2.getCameraInfo()); compare_calibration(exp, ci); } } @@ -471,21 +486,20 @@ TEST(SetInfo, saveCalibrationDefault) // Test ability to save calibrated CameraInfo to default location with // explicit camera name -TEST(SetInfo, saveCalibrationCameraName) +TEST_F(CameraInfoManagerTesting, saveCalibrationCameraName) { - ros::NodeHandle node; - sensor_msgs::CameraInfo exp(expected_calibration()); + sensor_msgs::msg::CameraInfo exp(expected_calibration()); bool success; // set ${ROS_HOME} to /tmp, delete the calibration file - std::string ros_home("/tmp"); - setenv("ROS_HOME", ros_home.c_str(), true); - std::string tmpFile(ros_home + "/camera_info/" + g_camera_name + ".yaml"); + rcpputils::set_env_var("ROS_HOME", "/tmp"); + + std::string tmpFile("/tmp/camera_info/" + g_camera_name + ".yaml"); delete_file(tmpFile); { // create instance to save calibrated data - camera_info_manager::CameraInfoManager cinfo(node, g_camera_name); + camera_info_manager::CameraInfoManager cinfo(node.get(), g_camera_name); success = set_calibration(node, exp); EXPECT_TRUE(cinfo.isCalibrated()); } @@ -494,60 +508,59 @@ TEST(SetInfo, saveCalibrationCameraName) // and redundant failure messages if (success) { // create a new instance to load saved calibration - camera_info_manager::CameraInfoManager cinfo2(node); + camera_info_manager::CameraInfoManager cinfo2(node.get()); std::string url = "file://" + tmpFile; cinfo2.loadCameraInfo(std::string(url)); EXPECT_TRUE(cinfo2.isCalibrated()); if (cinfo2.isCalibrated()) { - sensor_msgs::CameraInfo ci(cinfo2.getCameraInfo()); + sensor_msgs::msg::CameraInfo ci(cinfo2.getCameraInfo()); compare_calibration(exp, ci); } } -} - -// Test ability to save calibrated CameraInfo in a file -TEST(SetInfo, saveCalibrationFile) -{ - return; - - ros::NodeHandle node; - sensor_msgs::CameraInfo exp(expected_calibration()); - std::string cname("camera"); - std::string tmpFile("/tmp/camera_info_manager_calibration_test.yaml"); - std::string url = "file://" + tmpFile; - bool success; - - // first, delete the file delete_file(tmpFile); - - { - // create instance to save calibrated data - camera_info_manager::CameraInfoManager cinfo(node, cname, url); - success = set_calibration(node, exp); - EXPECT_TRUE(cinfo.isCalibrated()); - } - - // only check results if the service succeeded, avoiding confusing - // and redundant failure messages - if (success) { - // create a new instance to load saved calibration - camera_info_manager::CameraInfoManager cinfo2(node, cname, url); - EXPECT_TRUE(cinfo2.isCalibrated()); - if (cinfo2.isCalibrated()) { - sensor_msgs::CameraInfo ci(cinfo2.getCameraInfo()); - compare_calibration(exp, ci); - } - } } +// // Test ability to save calibrated CameraInfo in a file +// TEST_F(CameraInfoManagerTesting, saveCalibrationFile) +// { +// return; + +// ros::NodeHandle node; +// sensor_msgs::msg::CameraInfo exp(expected_calibration()); +// std::string cname("camera"); +// std::string tmpFile("/tmp/camera_info_manager_calibration_test.yaml"); +// std::string url = "file://" + tmpFile; +// bool success; + +// // first, delete the file +// delete_file(tmpFile); + +// { +// // create instance to save calibrated data +// camera_info_manager::CameraInfoManager cinfo(node, cname, url); +// success = set_calibration(node, exp); +// EXPECT_TRUE(cinfo.isCalibrated()); +// } + +// // only check results if the service succeeded, avoiding confusing +// // and redundant failure messages +// if (success) { +// // create a new instance to load saved calibration +// camera_info_manager::CameraInfoManager cinfo2(node, cname, url); +// EXPECT_TRUE(cinfo2.isCalibrated()); +// if (cinfo2.isCalibrated()) { +// sensor_msgs::msg::CameraInfo ci(cinfo2.getCameraInfo()); +// compare_calibration(exp, ci); +// } +// } +// } + // Test ability to save calibrated CameraInfo in a package // (needs write access). -TEST(SetInfo, saveCalibrationPackage) +TEST_F(CameraInfoManagerTesting, saveCalibrationPackage) { - ros::NodeHandle node; - sensor_msgs::CameraInfo exp(expected_calibration()); - std::string pkgPath(ros::package::getPath(g_package_name)); - std::string filename(pkgPath + g_package_filename); + sensor_msgs::msg::CameraInfo exp(expected_calibration()); + std::string filename(g_package_filename); bool success; // first, delete the file @@ -555,7 +568,7 @@ TEST(SetInfo, saveCalibrationPackage) { // create instance to save calibrated data - camera_info_manager::CameraInfoManager cinfo(node, g_camera_name, + camera_info_manager::CameraInfoManager cinfo(node.get(), g_camera_name, g_package_url); success = set_calibration(node, exp); EXPECT_TRUE(cinfo.isCalibrated()); @@ -565,44 +578,43 @@ TEST(SetInfo, saveCalibrationPackage) // and redundant failure messages if (success) { // create a new instance to load saved calibration - camera_info_manager::CameraInfoManager cinfo2(node, g_camera_name, + camera_info_manager::CameraInfoManager cinfo2(node.get(), g_camera_name, g_package_url); EXPECT_TRUE(cinfo2.isCalibrated()); if (cinfo2.isCalibrated()) { - sensor_msgs::CameraInfo ci(cinfo2.getCameraInfo()); + sensor_msgs::msg::CameraInfo ci(cinfo2.getCameraInfo()); compare_calibration(exp, ci); } } } -TEST(UrlSubstitution, cameraName) +TEST_F(CameraInfoManagerTesting, cameraName) { - ros::NodeHandle node; std::string name_url; std::string exp_url; // resolve a GUID camera name - name_url = "package://" + g_package_name + "/tests/${NAME}.yaml"; - exp_url = "package://" + g_package_name + "/tests/" + g_camera_name + ".yaml"; + name_url = +"/tests/${NAME}.yaml"; + exp_url = +"/tests/" + g_camera_name + ".yaml"; check_url_substitution(node, name_url, exp_url, g_camera_name); // substitute camera name "test" - name_url = "package://" + g_package_name + "/tests/${NAME}_calibration.yaml"; + name_url = +"/tests/${NAME}_calibration.yaml"; std::string test_name("test"); - exp_url = "package://" + g_package_name + "/tests/" + test_name + + exp_url = +"/tests/" + test_name + "_calibration.yaml"; check_url_substitution(node, name_url, exp_url, test_name); // with an '_' in the name test_name = "camera_1024x768"; - exp_url = "package://" + g_package_name + "/tests/" + test_name + + exp_url = +"/tests/" + test_name + "_calibration.yaml"; check_url_substitution(node, name_url, exp_url, test_name); // substitute empty camera name - name_url = "package://" + g_package_name + "/tests/${NAME}_calibration.yaml"; + name_url = +"/tests/${NAME}_calibration.yaml"; std::string empty_name(""); - exp_url = "package://" + g_package_name + "/tests/" + empty_name + + exp_url = +"/tests/" + empty_name + "_calibration.yaml"; check_url_substitution(node, name_url, exp_url, empty_name); @@ -610,16 +622,14 @@ TEST(UrlSubstitution, cameraName) check_url_substitution(node, g_package_name_url, g_package_url, g_test_name); } -TEST(UrlSubstitution, rosHome) +TEST_F(CameraInfoManagerTesting, rosHome) { - ros::NodeHandle node; std::string name_url; std::string exp_url; - char * home_env = getenv("HOME"); - std::string home(home_env); + std::string home = rcpputils::get_env_var("HOME"); // resolve ${ROS_HOME} with environment variable undefined - unsetenv("ROS_HOME"); + rcpputils::set_env_var("ROS_HOME", ""); name_url = "file://${ROS_HOME}/camera_info/test_camera.yaml"; exp_url = "file://" + home + "/.ros/camera_info/test_camera.yaml"; check_url_substitution(node, name_url, exp_url, g_camera_name); @@ -631,10 +641,8 @@ TEST(UrlSubstitution, rosHome) check_url_substitution(node, name_url, exp_url, g_camera_name); } -TEST(UrlSubstitution, unmatchedDollarSigns) +TEST_F(CameraInfoManagerTesting, unmatchedDollarSigns) { - ros::NodeHandle node; - // test for "$$" in the URL (NAME should be resolved) std::string name_url("file:///tmp/$${NAME}.yaml"); std::string exp_url("file:///tmp/$" + g_camera_name + ".yaml"); @@ -653,17 +661,16 @@ TEST(UrlSubstitution, unmatchedDollarSigns) check_url_substitution(node, name_url, name_url, g_camera_name); } -TEST(UrlSubstitution, emptyURL) +TEST_F(CameraInfoManagerTesting, emptyURL) { // test that empty URL is handled correctly - ros::NodeHandle node; + std::string empty_url(""); check_url_substitution(node, empty_url, empty_url, g_camera_name); } -TEST(UrlSubstitution, invalidVariables) +TEST_F(CameraInfoManagerTesting, invalidVariables) { - ros::NodeHandle node; std::string name_url; // missing "{...}" @@ -687,16 +694,14 @@ TEST(UrlSubstitution, invalidVariables) check_url_substitution(node, name_url, name_url, g_camera_name); } -// Run all the tests that were declared with TEST() +// Run all the tests that were declared with TEST_F() int main(int argc, char ** argv) { - ros::init(argc, argv, "camera_info_manager_unit_test"); testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); - // create asynchronous thread for handling service requests - ros::AsyncSpinner spinner(1); - spinner.start(); + auto ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); - // run the tests in this thread - return RUN_ALL_TESTS(); + return ret; } diff --git a/camera_info_manager/tests/unit_test.test b/camera_info_manager/tests/unit_test.test deleted file mode 100644 index e70b5736..00000000 --- a/camera_info_manager/tests/unit_test.test +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - From 29e65970c5d97471216cc96962b111bc16d6ee7e Mon Sep 17 00:00:00 2001 From: Alejandro Hernandez Cordero Date: Thu, 29 May 2025 10:46:05 +0200 Subject: [PATCH 2/5] Fixed windows build Signed-off-by: Alejandro Hernandez Cordero --- camera_info_manager/tests/unit_test.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/camera_info_manager/tests/unit_test.cpp b/camera_info_manager/tests/unit_test.cpp index 06ac0856..f6cb1cc3 100644 --- a/camera_info_manager/tests/unit_test.cpp +++ b/camera_info_manager/tests/unit_test.cpp @@ -29,7 +29,9 @@ #include +#ifndef _WIN32 #include +#endif #include #include From c06a99468a5a1fd1c8ce72cdfb884403bd7dea58 Mon Sep 17 00:00:00 2001 From: Alejandro Hernandez Cordero Date: Thu, 29 May 2025 17:04:43 +0200 Subject: [PATCH 3/5] windows build Signed-off-by: Alejandro Hernandez Cordero --- camera_info_manager/tests/unit_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/camera_info_manager/tests/unit_test.cpp b/camera_info_manager/tests/unit_test.cpp index f6cb1cc3..38f4939b 100644 --- a/camera_info_manager/tests/unit_test.cpp +++ b/camera_info_manager/tests/unit_test.cpp @@ -637,7 +637,7 @@ TEST_F(CameraInfoManagerTesting, rosHome) check_url_substitution(node, name_url, exp_url, g_camera_name); // resolve ${ROS_HOME} with environment variable defined - setenv("ROS_HOME", "/my/ros/home", true); + rcpputils::set_env_var("ROS_HOME", "/my/ros/home"); name_url = "file://${ROS_HOME}/camera_info/test_camera.yaml"; exp_url = "file:///my/ros/home/camera_info/test_camera.yaml"; check_url_substitution(node, name_url, exp_url, g_camera_name); From d2bd1ee825f178819f000729231c8bcc47478ca3 Mon Sep 17 00:00:00 2001 From: Alejandro Hernandez Cordero Date: Fri, 30 May 2025 13:26:15 +0200 Subject: [PATCH 4/5] Fixed windows build Signed-off-by: Alejandro Hernandez Cordero --- camera_info_manager/tests/unit_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/camera_info_manager/tests/unit_test.cpp b/camera_info_manager/tests/unit_test.cpp index 38f4939b..5f727bfb 100644 --- a/camera_info_manager/tests/unit_test.cpp +++ b/camera_info_manager/tests/unit_test.cpp @@ -317,7 +317,7 @@ TEST_F(CameraInfoManagerTesting, calibrated) camera_info_manager::CameraInfoManager cinfo(node.get()); EXPECT_FALSE(cinfo.isCalibrated()); - std::string current_path = std::filesystem::current_path(); + std::string current_path = std::filesystem::current_path().string(); std::string url("file://" + current_path + "/tests/test_calibration.yaml"); EXPECT_TRUE(cinfo.loadCameraInfo(url)); EXPECT_TRUE(cinfo.isCalibrated()); From beb21063b10ee31236702a3664d9bafc224d6245 Mon Sep 17 00:00:00 2001 From: Alejandro Hernandez Cordero Date: Wed, 4 Jun 2025 16:21:10 +0200 Subject: [PATCH 5/5] Try to fix windows test errors Signed-off-by: Alejandro Hernandez Cordero --- camera_info_manager/tests/unit_test.cpp | 26 +++++++++++++++++++++---- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/camera_info_manager/tests/unit_test.cpp b/camera_info_manager/tests/unit_test.cpp index 5f727bfb..30fd1ab2 100644 --- a/camera_info_manager/tests/unit_test.cpp +++ b/camera_info_manager/tests/unit_test.cpp @@ -447,9 +447,15 @@ TEST_F(CameraInfoManagerTesting, setCalibration) compare_calibration(exp, ci); } +#ifdef _WIN32 + std::string localdata; + localdata = rcpputils::get_env_var("localdata"); + delete_file(localdata + "/ros/camera_info/camera.yaml"); +#else std::string home; home = rcpputils::get_env_var("HOME"); delete_file(home + "/.ros/camera_info/camera.yaml"); +#endif } // Test ability to save calibrated CameraInfo in default URL @@ -496,9 +502,15 @@ TEST_F(CameraInfoManagerTesting, saveCalibrationCameraName) // set ${ROS_HOME} to /tmp, delete the calibration file rcpputils::set_env_var("ROS_HOME", "/tmp"); - std::string tmpFile("/tmp/camera_info/" + g_camera_name + ".yaml"); + std::string tmpFile; +#ifdef _WIN32 + std::string localdata; + localdata = rcpputils::get_env_var("localdata"); + tmpFile = std::string(localdata + "/camera_info/" + g_camera_name + ".yaml"); +#else + tmpFile = std::string("/tmp/camera_info/" + g_camera_name + ".yaml"); +#endif delete_file(tmpFile); - { // create instance to save calibrated data camera_info_manager::CameraInfoManager cinfo(node.get(), g_camera_name); @@ -628,14 +640,20 @@ TEST_F(CameraInfoManagerTesting, rosHome) { std::string name_url; std::string exp_url; - std::string home = rcpputils::get_env_var("HOME"); // resolve ${ROS_HOME} with environment variable undefined rcpputils::set_env_var("ROS_HOME", ""); name_url = "file://${ROS_HOME}/camera_info/test_camera.yaml"; +#ifdef _WIN32 + std::string localdata; + localdata = rcpputils::get_env_var("localdata"); + exp_url = "file://" + localdata + "/ros/camera_info/test_camera.yaml"; + check_url_substitution(node, name_url, exp_url, g_camera_name); +#else + std::string home = rcpputils::get_env_var("HOME"); exp_url = "file://" + home + "/.ros/camera_info/test_camera.yaml"; check_url_substitution(node, name_url, exp_url, g_camera_name); - +#endif // resolve ${ROS_HOME} with environment variable defined rcpputils::set_env_var("ROS_HOME", "/my/ros/home"); name_url = "file://${ROS_HOME}/camera_info/test_camera.yaml";