00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <unistd.h>
00038 #include <stdlib.h>
00039 #include <ros/ros.h>
00040 #include <ros/package.h>
00041 #include "camera_info_manager/camera_info_manager.h"
00042 #include <sensor_msgs/distortion_models.h>
00043 #include <string>
00044 #include <gtest/gtest.h>
00045
00047
00049
00050 namespace
00051 {
00052 const std::string g_package_name("camera_info_manager");
00053 const std::string g_test_name("test_calibration");
00054 const std::string g_package_filename("/tests/" + g_test_name +".yaml");
00055 const std::string g_package_url("package://" + g_package_name
00056 + g_package_filename);
00057 const std::string g_package_name_url("package://" + g_package_name
00058 + "/tests/${NAME}.yaml");
00059 const std::string g_default_url("file://${ROS_HOME}/camera_info/${NAME}.yaml");
00060 const std::string g_camera_name("08144361026320a0");
00061 }
00062
00064
00066
00067
00068 void compare_calibration(const sensor_msgs::CameraInfo &exp,
00069 const sensor_msgs::CameraInfo &ci)
00070 {
00071
00072 EXPECT_EQ(exp.width, ci.width);
00073 EXPECT_EQ(exp.height, ci.height);
00074
00075
00076 EXPECT_EQ(exp.distortion_model, ci.distortion_model);
00077 EXPECT_EQ(exp.D.size(), ci.D.size());
00078 for (unsigned i = 0; i < ci.D.size(); ++i)
00079 {
00080 EXPECT_EQ(exp.D[i], ci.D[i]);
00081 }
00082
00083
00084 for (unsigned i = 0; i < ci.K.size(); ++i)
00085 {
00086 EXPECT_EQ(exp.K[i], ci.K[i]);
00087 }
00088
00089
00090 for (unsigned i = 0; i < ci.R.size(); ++i)
00091 {
00092 EXPECT_EQ(exp.R[i], ci.R[i]);
00093 }
00094
00095
00096 for (unsigned i = 0; i < ci.P.size(); ++i)
00097 {
00098 EXPECT_EQ(exp.P[i], ci.P[i]);
00099 }
00100 }
00101
00102
00103 void delete_file(std::string filename)
00104 {
00105 int rc = unlink(filename.c_str());
00106 if (rc != 0)
00107 {
00108 if (errno != ENOENT)
00109 ROS_INFO_STREAM("unexpected unlink() error: " << errno);
00110 }
00111 }
00112
00113 void delete_default_file(void)
00114 {
00115 std::string ros_home("/tmp");
00116 setenv("ROS_HOME", ros_home.c_str(), true);
00117 std::string tmpFile(ros_home + "/camera_info/camera.yaml");
00118 delete_file(tmpFile);
00119 }
00120
00121 void do_system(const std::string &command)
00122 {
00123 int rc = system(command.c_str());
00124 if (rc)
00125 std::cout << command << " returns " << rc;
00126 }
00127
00128 void delete_tmp_camera_info_directory(void)
00129 {
00130 do_system(std::string("rm -rf /tmp/camera_info"));
00131 }
00132
00133 void make_tmp_camera_info_directory(void)
00134 {
00135 do_system(std::string("mkdir -p /tmp/camera_info"));
00136 }
00137
00138
00139 sensor_msgs::CameraInfo expected_calibration(void)
00140 {
00141 sensor_msgs::CameraInfo ci;
00142
00143 ci.width = 640u;
00144 ci.height = 480u;
00145
00146
00147 ci.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
00148 ci.D.resize(5);
00149 ci.D[0] = -1.04482;
00150 ci.D[1] = 1.59252;
00151 ci.D[2] = -0.0196308;
00152 ci.D[3] = 0.0287906;
00153 ci.D[4] = 0.0;
00154
00155
00156 ci.K[0] = 1168.68;
00157 ci.K[1] = 0.0;
00158 ci.K[2] = 295.015;
00159 ci.K[3] = 0.0;
00160 ci.K[4] = 1169.01;
00161 ci.K[5] = 252.247;
00162 ci.K[6] = 0.0;
00163 ci.K[7] = 0.0;
00164 ci.K[8] = 1.0;
00165
00166
00167 ci.R[0] = 1.0;
00168 ci.R[1] = 0.0;
00169 ci.R[2] = 0.0;
00170 ci.R[3] = 0.0;
00171 ci.R[4] = 1.0;
00172 ci.R[5] = 0.0;
00173 ci.R[6] = 0.0;
00174 ci.R[7] = 0.0;
00175 ci.R[8] = 1.0;
00176
00177
00178 ci.P[0] = ci.K[0];
00179 ci.P[1] = ci.K[1];
00180 ci.P[2] = ci.K[2];
00181 ci.P[3] = 0.0;
00182 ci.P[4] = ci.K[3];
00183 ci.P[5] = ci.K[4];
00184 ci.P[6] = ci.K[5];
00185 ci.P[7] = 0.0;
00186 ci.P[8] = ci.K[6];
00187 ci.P[9] = ci.K[7];
00188 ci.P[10] = ci.K[8];
00189 ci.P[11] = 0.0;
00190
00191 return ci;
00192 }
00193
00194
00195 bool set_calibration(ros::NodeHandle node,
00196 const sensor_msgs::CameraInfo &calib)
00197 {
00198 ros::ServiceClient client =
00199 node.serviceClient<sensor_msgs::SetCameraInfo>("set_camera_info");
00200 sensor_msgs::SetCameraInfo set_camera_info;
00201 set_camera_info.request.camera_info = calib;
00202 bool success;
00203 EXPECT_TRUE((success = client.call(set_camera_info)));
00204 return success;
00205 }
00206
00207
00208 void check_url_substitution(ros::NodeHandle node,
00209 const std::string &url,
00210 const std::string &exp_url,
00211 const std::string &camera_name)
00212 {
00213 camera_info_manager::CameraInfoManager cinfo(node, camera_name, url);
00214 std::string sub_url = cinfo.resolveURL(url, camera_name);
00215 EXPECT_EQ(sub_url, exp_url);
00216 }
00217
00219
00221
00222
00223 TEST(CameraName, validNames)
00224 {
00225 ros::NodeHandle node;
00226 camera_info_manager::CameraInfoManager cinfo(node);
00227
00228 EXPECT_TRUE(cinfo.setCameraName(std::string("a")));
00229 EXPECT_TRUE(cinfo.setCameraName(std::string("1")));
00230 EXPECT_TRUE(cinfo.setCameraName(std::string("_")));
00231 EXPECT_TRUE(cinfo.setCameraName(std::string("abcdefghijklmnopqrstuvwxyz")));
00232 EXPECT_TRUE(cinfo.setCameraName(std::string("ABCDEFGHIJKLMNOPQRSTUVWXYZ")));
00233 EXPECT_TRUE(cinfo.setCameraName(std::string("0123456789")));
00234 EXPECT_TRUE(cinfo.setCameraName(std::string("0123456789abcdef")));
00235 EXPECT_TRUE(cinfo.setCameraName(std::string("A1")));
00236 EXPECT_TRUE(cinfo.setCameraName(std::string("9z")));
00237 EXPECT_TRUE(cinfo.setCameraName(std::string("08144361026320a0_640x480_mono8")));
00238
00239 }
00240
00241
00242 TEST(CameraName, invalidNames)
00243 {
00244 ros::NodeHandle node;
00245 camera_info_manager::CameraInfoManager cinfo(node);
00246
00247 EXPECT_FALSE(cinfo.setCameraName(std::string("")));
00248 EXPECT_FALSE(cinfo.setCameraName(std::string("-21")));
00249 EXPECT_FALSE(cinfo.setCameraName(std::string("C++")));
00250 EXPECT_FALSE(cinfo.setCameraName(std::string("file:///tmp/url.yaml")));
00251 EXPECT_FALSE(cinfo.setCameraName(std::string("file://${INVALID}/xxx.yaml")));
00252 }
00253
00254
00255 TEST(UrlValidation, validURLs)
00256 {
00257 ros::NodeHandle node;
00258 camera_info_manager::CameraInfoManager cinfo(node);
00259
00260 EXPECT_TRUE(cinfo.validateURL(std::string("")));
00261 EXPECT_TRUE(cinfo.validateURL(std::string("file:///")));
00262 EXPECT_TRUE(cinfo.validateURL(std::string("file:///tmp/url.yaml")));
00263 EXPECT_TRUE(cinfo.validateURL(std::string("File:///tmp/url.ini")));
00264 EXPECT_TRUE(cinfo.validateURL(std::string("FILE:///tmp/url.yaml")));
00265 EXPECT_TRUE(cinfo.validateURL(g_default_url));
00266 EXPECT_TRUE(cinfo.validateURL(g_package_url));
00267 EXPECT_TRUE(cinfo.validateURL(std::string("package://no_such_package/calibration.yaml")));
00268 EXPECT_TRUE(cinfo.validateURL(std::string("packAge://camera_info_manager/x")));
00269 }
00270
00271
00272 TEST(UrlValidation, invalidURLs)
00273 {
00274 ros::NodeHandle node;
00275 camera_info_manager::CameraInfoManager cinfo(node);
00276
00277 EXPECT_FALSE(cinfo.validateURL(std::string("file://")));
00278 EXPECT_FALSE(cinfo.validateURL(std::string("flash:///")));
00279 EXPECT_FALSE(cinfo.validateURL(std::string("html://ros.org/wiki/camera_info_manager")));
00280 EXPECT_FALSE(cinfo.validateURL(std::string("package://")));
00281 EXPECT_FALSE(cinfo.validateURL(std::string("package:///")));
00282 EXPECT_FALSE(cinfo.validateURL(std::string("package://calibration.yaml")));
00283 EXPECT_FALSE(cinfo.validateURL(std::string("package://camera_info_manager/")));
00284 }
00285
00286
00287 TEST(GetInfo, uncalibrated)
00288 {
00289 ros::NodeHandle node;
00290
00291 delete_default_file();
00292
00293 camera_info_manager::CameraInfoManager cinfo(node);
00294 EXPECT_FALSE(cinfo.isCalibrated());
00295
00296 sensor_msgs::CameraInfo ci(cinfo.getCameraInfo());
00297 sensor_msgs::CameraInfo exp;
00298 compare_calibration(exp, ci);
00299 }
00300
00301
00302 TEST(GetInfo, calibrated)
00303 {
00304 ros::NodeHandle node;
00305
00306 delete_default_file();
00307
00308 camera_info_manager::CameraInfoManager cinfo(node);
00309 EXPECT_FALSE(cinfo.isCalibrated());
00310
00311 std::string pkgPath(ros::package::getPath(g_package_name));
00312 std::string url("file://" + pkgPath + "/tests/test_calibration.yaml");
00313 EXPECT_TRUE(cinfo.loadCameraInfo(url));
00314 EXPECT_TRUE(cinfo.isCalibrated());
00315
00316 sensor_msgs::CameraInfo ci(cinfo.getCameraInfo());
00317 sensor_msgs::CameraInfo exp(expected_calibration());
00318 compare_calibration(exp, ci);
00319 }
00320
00321
00322 TEST(GetInfo, fromPackage)
00323 {
00324 ros::NodeHandle node;
00325 camera_info_manager::CameraInfoManager cinfo(node);
00326
00327 EXPECT_TRUE(cinfo.loadCameraInfo(g_package_url));
00328 EXPECT_TRUE(cinfo.isCalibrated());
00329
00330 sensor_msgs::CameraInfo ci(cinfo.getCameraInfo());
00331 sensor_msgs::CameraInfo exp(expected_calibration());
00332 compare_calibration(exp, ci);
00333 }
00334
00335
00336 TEST(GetInfo, fromPackageWithName)
00337 {
00338 ros::NodeHandle node;
00339 camera_info_manager::CameraInfoManager cinfo(node, g_test_name,
00340 g_package_name_url);
00341 EXPECT_TRUE(cinfo.isCalibrated());
00342
00343 sensor_msgs::CameraInfo ci(cinfo.getCameraInfo());
00344 sensor_msgs::CameraInfo exp(expected_calibration());
00345 compare_calibration(exp, ci);
00346 }
00347
00348
00349 TEST(GetInfo, unresolvedLoads)
00350 {
00351 ros::NodeHandle node;
00352 camera_info_manager::CameraInfoManager cinfo(node);
00353
00354 EXPECT_FALSE(cinfo.loadCameraInfo(std::string("package://")));
00355 EXPECT_FALSE(cinfo.isCalibrated());
00356
00357 EXPECT_FALSE(cinfo.loadCameraInfo(std::string("package:///calibration.yaml")));
00358 EXPECT_FALSE(cinfo.isCalibrated());
00359
00360 EXPECT_FALSE(cinfo.loadCameraInfo(std::string("package://no_such_package/calibration.yaml")));
00361 EXPECT_FALSE(cinfo.isCalibrated());
00362
00363 EXPECT_FALSE(cinfo.loadCameraInfo(std::string("package://camera_info_manager/no_such_file.yaml")));
00364 EXPECT_FALSE(cinfo.isCalibrated());
00365 }
00366
00367
00368 TEST(GetInfo, nameChange)
00369 {
00370 ros::NodeHandle node;
00371 const std::string missing_file("no_such_file");
00372
00373
00374 camera_info_manager::CameraInfoManager cinfo(node, missing_file,
00375 g_package_name_url);
00376 EXPECT_FALSE(cinfo.isCalibrated());
00377
00378
00379 EXPECT_TRUE(cinfo.setCameraName(g_test_name));
00380 EXPECT_TRUE(cinfo.isCalibrated());
00381 sensor_msgs::CameraInfo ci(cinfo.getCameraInfo());
00382 sensor_msgs::CameraInfo exp(expected_calibration());
00383 compare_calibration(exp, ci);
00384 }
00385
00386
00387 TEST(GetInfo, invalidLoads)
00388 {
00389 ros::NodeHandle node;
00390 camera_info_manager::CameraInfoManager cinfo(node);
00391
00392 EXPECT_FALSE(cinfo.loadCameraInfo(std::string("flash:///")));
00393 EXPECT_FALSE(cinfo.isCalibrated());
00394
00395 EXPECT_FALSE(cinfo.loadCameraInfo(std::string("html://ros.org/wiki/camera_info_manager")));
00396 EXPECT_FALSE(cinfo.isCalibrated());
00397
00398 sensor_msgs::CameraInfo ci(cinfo.getCameraInfo());
00399 sensor_msgs::CameraInfo exp;
00400 compare_calibration(exp, ci);
00401 }
00402
00403
00404 TEST(SetInfo, setCalibration)
00405 {
00406 ros::NodeHandle node;
00407 camera_info_manager::CameraInfoManager cinfo(node);
00408
00409
00410 sensor_msgs::CameraInfo exp(expected_calibration());
00411 bool success = set_calibration(node, exp);
00412
00413
00414
00415 if (success)
00416 {
00417
00418 EXPECT_TRUE(cinfo.isCalibrated());
00419 sensor_msgs::CameraInfo ci = cinfo.getCameraInfo();
00420 compare_calibration(exp, ci);
00421 }
00422 }
00423
00424
00425 TEST(SetInfo, saveCalibrationDefault)
00426 {
00427 ros::NodeHandle node;
00428 sensor_msgs::CameraInfo exp(expected_calibration());
00429 bool success;
00430
00431
00432
00433 setenv("ROS_HOME", "/tmp", true);
00434 delete_tmp_camera_info_directory();
00435
00436 {
00437
00438 camera_info_manager::CameraInfoManager cinfo(node);
00439 EXPECT_FALSE(cinfo.isCalibrated());
00440
00441
00442 success = set_calibration(node, exp);
00443 EXPECT_TRUE(cinfo.isCalibrated());
00444 }
00445
00446
00447
00448 if (success)
00449 {
00450
00451 camera_info_manager::CameraInfoManager cinfo2(node);
00452 EXPECT_TRUE(cinfo2.isCalibrated());
00453 if (cinfo2.isCalibrated())
00454 {
00455 sensor_msgs::CameraInfo ci(cinfo2.getCameraInfo());
00456 compare_calibration(exp, ci);
00457 }
00458 }
00459 }
00460
00461
00462
00463 TEST(SetInfo, saveCalibrationCameraName)
00464 {
00465 ros::NodeHandle node;
00466 sensor_msgs::CameraInfo exp(expected_calibration());
00467 bool success;
00468
00469
00470 std::string ros_home("/tmp");
00471 setenv("ROS_HOME", ros_home.c_str(), true);
00472 std::string tmpFile(ros_home + "/camera_info/" + g_camera_name + ".yaml");
00473 delete_file(tmpFile);
00474
00475 {
00476
00477 camera_info_manager::CameraInfoManager cinfo(node, g_camera_name);
00478 success = set_calibration(node, exp);
00479 EXPECT_TRUE(cinfo.isCalibrated());
00480 }
00481
00482
00483
00484 if (success)
00485 {
00486
00487 camera_info_manager::CameraInfoManager cinfo2(node);
00488 std::string url = "file://" + tmpFile;
00489 cinfo2.loadCameraInfo(std::string(url));
00490 EXPECT_TRUE(cinfo2.isCalibrated());
00491 if (cinfo2.isCalibrated())
00492 {
00493 sensor_msgs::CameraInfo ci(cinfo2.getCameraInfo());
00494 compare_calibration(exp, ci);
00495 }
00496 }
00497 }
00498
00499
00500 TEST(SetInfo, saveCalibrationFile)
00501 {
00502 return;
00503
00504 ros::NodeHandle node;
00505 sensor_msgs::CameraInfo exp(expected_calibration());
00506 std::string cname("camera");
00507 std::string tmpFile("/tmp/camera_info_manager_calibration_test.yaml");
00508 std::string url = "file://" + tmpFile;
00509 bool success;
00510
00511
00512 delete_file(tmpFile);
00513
00514 {
00515
00516 camera_info_manager::CameraInfoManager cinfo(node, cname, url);
00517 success = set_calibration(node, exp);
00518 EXPECT_TRUE(cinfo.isCalibrated());
00519 }
00520
00521
00522
00523 if (success)
00524 {
00525
00526 camera_info_manager::CameraInfoManager cinfo2(node, cname, url);
00527 EXPECT_TRUE(cinfo2.isCalibrated());
00528 if (cinfo2.isCalibrated())
00529 {
00530 sensor_msgs::CameraInfo ci(cinfo2.getCameraInfo());
00531 compare_calibration(exp, ci);
00532 }
00533 }
00534 }
00535
00536
00537
00538 TEST(SetInfo, saveCalibrationPackage)
00539 {
00540 ros::NodeHandle node;
00541 sensor_msgs::CameraInfo exp(expected_calibration());
00542 std::string pkgPath(ros::package::getPath(g_package_name));
00543 std::string filename(pkgPath + g_package_filename);
00544 bool success;
00545
00546
00547 delete_file(filename);
00548
00549 {
00550
00551 camera_info_manager::CameraInfoManager cinfo(node, g_camera_name,
00552 g_package_url);
00553 success = set_calibration(node, exp);
00554 EXPECT_TRUE(cinfo.isCalibrated());
00555 }
00556
00557
00558
00559 if (success)
00560 {
00561
00562 camera_info_manager::CameraInfoManager cinfo2(node, g_camera_name,
00563 g_package_url);
00564 EXPECT_TRUE(cinfo2.isCalibrated());
00565 if (cinfo2.isCalibrated())
00566 {
00567 sensor_msgs::CameraInfo ci(cinfo2.getCameraInfo());
00568 compare_calibration(exp, ci);
00569 }
00570 }
00571 }
00572
00573 TEST(UrlSubstitution, cameraName)
00574 {
00575 ros::NodeHandle node;
00576 std::string name_url;
00577 std::string exp_url;
00578
00579
00580 name_url = "package://" + g_package_name + "/tests/${NAME}.yaml";
00581 exp_url = "package://" + g_package_name + "/tests/" + g_camera_name + ".yaml";
00582 check_url_substitution(node, name_url, exp_url, g_camera_name);
00583
00584
00585 name_url = "package://" + g_package_name + "/tests/${NAME}_calibration.yaml";
00586 std::string test_name("test");
00587 exp_url = "package://" + g_package_name + "/tests/" + test_name
00588 + "_calibration.yaml";
00589 check_url_substitution(node, name_url, exp_url, test_name);
00590
00591
00592 test_name = "camera_1024x768";
00593 exp_url = "package://" + g_package_name + "/tests/" + test_name
00594 + "_calibration.yaml";
00595 check_url_substitution(node, name_url, exp_url, test_name);
00596
00597
00598 name_url = "package://" + g_package_name + "/tests/${NAME}_calibration.yaml";
00599 std::string empty_name("");
00600 exp_url = "package://" + g_package_name + "/tests/" + empty_name
00601 + "_calibration.yaml";
00602 check_url_substitution(node, name_url, exp_url, empty_name);
00603
00604
00605 check_url_substitution(node, g_package_name_url, g_package_url, g_test_name);
00606 }
00607
00608 TEST(UrlSubstitution, rosHome)
00609 {
00610 ros::NodeHandle node;
00611 std::string name_url;
00612 std::string exp_url;
00613 char *home_env = getenv("HOME");
00614 std::string home(home_env);
00615
00616
00617 unsetenv("ROS_HOME");
00618 name_url = "file://${ROS_HOME}/camera_info/test_camera.yaml";
00619 exp_url = "file://" + home + "/.ros/camera_info/test_camera.yaml";
00620 check_url_substitution(node, name_url, exp_url, g_camera_name);
00621
00622
00623 setenv("ROS_HOME", "/my/ros/home", true);
00624 name_url = "file://${ROS_HOME}/camera_info/test_camera.yaml";
00625 exp_url = "file:///my/ros/home/camera_info/test_camera.yaml";
00626 check_url_substitution(node, name_url, exp_url, g_camera_name);
00627 }
00628
00629 TEST(UrlSubstitution, unmatchedDollarSigns)
00630 {
00631 ros::NodeHandle node;
00632
00633
00634 std::string name_url("file:///tmp/$${NAME}.yaml");
00635 std::string exp_url("file:///tmp/$" + g_camera_name + ".yaml");
00636 check_url_substitution(node, name_url, exp_url, g_camera_name);
00637
00638
00639 name_url = "file:///$whatever.yaml";
00640 check_url_substitution(node, name_url, name_url, g_camera_name);
00641
00642
00643 name_url = "file:///something$$whatever.yaml";
00644 check_url_substitution(node, name_url, name_url, g_camera_name);
00645
00646
00647 name_url = "file:///$$";
00648 check_url_substitution(node, name_url, name_url, g_camera_name);
00649 }
00650
00651 TEST(UrlSubstitution, emptyURL)
00652 {
00653
00654 ros::NodeHandle node;
00655 std::string empty_url("");
00656 check_url_substitution(node, empty_url, empty_url, g_camera_name);
00657 }
00658
00659 TEST(UrlSubstitution, invalidVariables)
00660 {
00661 ros::NodeHandle node;
00662 std::string name_url;
00663
00664
00665 name_url = "file:///tmp/$NAME.yaml";
00666 check_url_substitution(node, name_url, name_url, g_camera_name);
00667
00668
00669 name_url = "file:///tmp/${INVALID}/calibration.yaml";
00670 check_url_substitution(node, name_url, name_url, g_camera_name);
00671
00672
00673 name_url = "file:///tmp/${NAME";
00674 check_url_substitution(node, name_url, name_url, g_camera_name);
00675
00676
00677 name_url = "file:///tmp/${}";
00678 check_url_substitution(node, name_url, name_url, g_camera_name);
00679
00680
00681 name_url = "file:///$";
00682 check_url_substitution(node, name_url, name_url, g_camera_name);
00683 }
00684
00685
00686
00687 TEST(DeprecatedClass, compileWarning)
00688 {
00689 ros::NodeHandle node;
00690 CameraInfoManager cinfo(node);
00691 }
00692
00693
00694 int main(int argc, char **argv)
00695 {
00696 ros::init(argc, argv, "camera_info_manager_unit_test");
00697 testing::InitGoogleTest(&argc, argv);
00698
00699
00700 ros::AsyncSpinner spinner(1);
00701 spinner.start();
00702
00703
00704 return RUN_ALL_TESTS();
00705 }