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 <ros/ros.h>
00039 #include <ros/package.h>
00040 #include "camera_info_manager/camera_info_manager.h"
00041 #include <sensor_msgs/distortion_models.h>
00042 #include <string>
00043 #include <gtest/gtest.h>
00044
00046
00048
00049 namespace
00050 {
00051 const std::string g_package_name("camera_info_manager");
00052 const std::string g_package_filename("/tests/test_calibration.yaml");
00053 const std::string g_package_url("package://" + g_package_name
00054 + "/" + g_package_filename);
00055 const std::string g_camera_name("08144361026320a0");
00056 }
00057
00059
00061
00062
00063 void compare_calibration(const sensor_msgs::CameraInfo &exp,
00064 const sensor_msgs::CameraInfo &ci)
00065 {
00066
00067 EXPECT_EQ(exp.width, ci.width);
00068 EXPECT_EQ(exp.height, ci.height);
00069
00070
00071 EXPECT_EQ(exp.distortion_model, ci.distortion_model);
00072 EXPECT_EQ(exp.D.size(), ci.D.size());
00073 for (unsigned i = 0; i < ci.D.size(); ++i)
00074 {
00075 EXPECT_EQ(exp.D[i], ci.D[i]);
00076 }
00077
00078
00079 for (unsigned i = 0; i < ci.K.size(); ++i)
00080 {
00081 EXPECT_EQ(exp.K[i], ci.K[i]);
00082 }
00083
00084
00085 for (unsigned i = 0; i < ci.R.size(); ++i)
00086 {
00087 EXPECT_EQ(exp.R[i], ci.R[i]);
00088 }
00089
00090
00091 for (unsigned i = 0; i < ci.P.size(); ++i)
00092 {
00093 EXPECT_EQ(exp.P[i], ci.P[i]);
00094 }
00095 }
00096
00097
00098 void delete_file(std::string filename)
00099 {
00100 int rc = unlink(filename.c_str());
00101 if (rc != 0)
00102 {
00103 EXPECT_EQ(errno, ENOENT);
00104 }
00105 }
00106
00107
00108 sensor_msgs::CameraInfo expected_calibration(void)
00109 {
00110 sensor_msgs::CameraInfo ci;
00111
00112 ci.width = 640u;
00113 ci.height = 480u;
00114
00115
00116 ci.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
00117 ci.D.resize(5);
00118 ci.D[0] = -1.04482;
00119 ci.D[1] = 1.59252;
00120 ci.D[2] = -0.0196308;
00121 ci.D[3] = 0.0287906;
00122 ci.D[4] = 0.0;
00123
00124
00125 ci.K[0] = 1168.68;
00126 ci.K[1] = 0.0;
00127 ci.K[2] = 295.015;
00128 ci.K[3] = 0.0;
00129 ci.K[4] = 1169.01;
00130 ci.K[5] = 252.247;
00131 ci.K[6] = 0.0;
00132 ci.K[7] = 0.0;
00133 ci.K[8] = 1.0;
00134
00135
00136 ci.R[0] = 1.0;
00137 ci.R[1] = 0.0;
00138 ci.R[2] = 0.0;
00139 ci.R[3] = 0.0;
00140 ci.R[4] = 1.0;
00141 ci.R[5] = 0.0;
00142 ci.R[6] = 0.0;
00143 ci.R[7] = 0.0;
00144 ci.R[8] = 1.0;
00145
00146
00147 ci.P[0] = ci.K[0];
00148 ci.P[1] = ci.K[1];
00149 ci.P[2] = ci.K[2];
00150 ci.P[3] = 0.0;
00151 ci.P[4] = ci.K[3];
00152 ci.P[5] = ci.K[4];
00153 ci.P[6] = ci.K[5];
00154 ci.P[7] = 0.0;
00155 ci.P[8] = ci.K[6];
00156 ci.P[9] = ci.K[7];
00157 ci.P[10] = ci.K[8];
00158 ci.P[11] = 0.0;
00159
00160 return ci;
00161 }
00162
00163
00164 bool set_calibration(ros::NodeHandle node,
00165 const sensor_msgs::CameraInfo &calib)
00166 {
00167 ros::ServiceClient client =
00168 node.serviceClient<sensor_msgs::SetCameraInfo>("set_camera_info");
00169 sensor_msgs::SetCameraInfo set_camera_info;
00170 set_camera_info.request.camera_info = calib;
00171 bool success;
00172 EXPECT_TRUE((success = client.call(set_camera_info)));
00173 return success;
00174 }
00175
00177
00179
00180
00181 TEST(CameraName, validNames)
00182 {
00183 ros::NodeHandle node;
00184 CameraInfoManager cinfo(node);
00185
00186 EXPECT_TRUE(cinfo.setCameraName(std::string("a")));
00187 EXPECT_TRUE(cinfo.setCameraName(std::string("1")));
00188 EXPECT_TRUE(cinfo.setCameraName(std::string("_")));
00189 EXPECT_TRUE(cinfo.setCameraName(std::string("abcdefghijklmnopqrstuvwxyz")));
00190 EXPECT_TRUE(cinfo.setCameraName(std::string("ABCDEFGHIJKLMNOPQRSTUVWXYZ")));
00191 EXPECT_TRUE(cinfo.setCameraName(std::string("0123456789")));
00192 EXPECT_TRUE(cinfo.setCameraName(std::string("0123456789abcdef")));
00193 EXPECT_TRUE(cinfo.setCameraName(std::string("A1")));
00194 EXPECT_TRUE(cinfo.setCameraName(std::string("9z")));
00195 }
00196
00197
00198 TEST(CameraName, invalidNames)
00199 {
00200 ros::NodeHandle node;
00201 CameraInfoManager cinfo(node);
00202
00203 EXPECT_FALSE(cinfo.setCameraName(std::string("")));
00204 EXPECT_FALSE(cinfo.setCameraName(std::string("-21")));
00205 EXPECT_FALSE(cinfo.setCameraName(std::string("C++")));
00206 EXPECT_FALSE(cinfo.setCameraName(std::string("file:///tmp/url.yaml")));
00207 }
00208
00209
00210 TEST(CameraName, validURLs)
00211 {
00212 ros::NodeHandle node;
00213 CameraInfoManager cinfo(node);
00214
00215 EXPECT_TRUE(cinfo.validateURL(std::string("")));
00216 EXPECT_TRUE(cinfo.validateURL(std::string("file:///")));
00217 EXPECT_TRUE(cinfo.validateURL(std::string("file:///tmp/url.yaml")));
00218 EXPECT_TRUE(cinfo.validateURL(std::string("file:///tmp/url.ini")));
00219 EXPECT_TRUE(cinfo.validateURL(g_package_url));
00220 EXPECT_TRUE(cinfo.validateURL(std::string("package://no_such_package/calibration.yaml")));
00221 EXPECT_TRUE(cinfo.validateURL(std::string("package://camera_info_manager/x")));
00222 }
00223
00224
00225 TEST(CameraName, invalidURLs)
00226 {
00227 ros::NodeHandle node;
00228 CameraInfoManager cinfo(node);
00229
00230 EXPECT_FALSE(cinfo.validateURL(std::string("file://")));
00231 EXPECT_FALSE(cinfo.validateURL(std::string("flash:///")));
00232 EXPECT_FALSE(cinfo.validateURL(std::string("html://ros.org/wiki/camera_info_manager")));
00233 EXPECT_FALSE(cinfo.validateURL(std::string("package://")));
00234 EXPECT_FALSE(cinfo.validateURL(std::string("package:///")));
00235 EXPECT_FALSE(cinfo.validateURL(std::string("package://calibration.yaml")));
00236 EXPECT_FALSE(cinfo.validateURL(std::string("package://camera_info_manager/")));
00237 }
00238
00239
00240 TEST(getInfo, uncalibrated)
00241 {
00242 ros::NodeHandle node;
00243 CameraInfoManager cinfo(node);
00244
00245 EXPECT_FALSE(cinfo.isCalibrated());
00246
00247 sensor_msgs::CameraInfo ci(cinfo.getCameraInfo());
00248 sensor_msgs::CameraInfo exp;
00249 compare_calibration(exp, ci);
00250 }
00251
00252
00253 TEST(getInfo, calibrated)
00254 {
00255 ros::NodeHandle node;
00256 CameraInfoManager cinfo(node);
00257
00258 EXPECT_FALSE(cinfo.isCalibrated());
00259 std::string pkgPath(ros::package::getPath(g_package_name));
00260 std::string url("file://" + pkgPath + "/tests/test_calibration.yaml");
00261 EXPECT_TRUE(cinfo.loadCameraInfo(url));
00262 EXPECT_TRUE(cinfo.isCalibrated());
00263
00264 sensor_msgs::CameraInfo ci(cinfo.getCameraInfo());
00265 sensor_msgs::CameraInfo exp(expected_calibration());
00266 compare_calibration(exp, ci);
00267 }
00268
00269
00270 TEST(getInfo, fromPackage)
00271 {
00272 ros::NodeHandle node;
00273 CameraInfoManager cinfo(node);
00274
00275 EXPECT_FALSE(cinfo.isCalibrated());
00276 EXPECT_TRUE(cinfo.loadCameraInfo(g_package_url));
00277 EXPECT_TRUE(cinfo.isCalibrated());
00278
00279 sensor_msgs::CameraInfo ci(cinfo.getCameraInfo());
00280 sensor_msgs::CameraInfo exp(expected_calibration());
00281 compare_calibration(exp, ci);
00282 }
00283
00284
00285 TEST(getInfo, unresolvedLoads)
00286 {
00287 ros::NodeHandle node;
00288 CameraInfoManager cinfo(node);
00289 EXPECT_FALSE(cinfo.isCalibrated());
00290
00291 EXPECT_FALSE(cinfo.loadCameraInfo(std::string("package://")));
00292 EXPECT_FALSE(cinfo.isCalibrated());
00293
00294 EXPECT_FALSE(cinfo.loadCameraInfo(std::string("package:///calibration.yaml")));
00295 EXPECT_FALSE(cinfo.isCalibrated());
00296
00297 EXPECT_FALSE(cinfo.loadCameraInfo(std::string("package://no_such_package/calibration.yaml")));
00298 EXPECT_FALSE(cinfo.isCalibrated());
00299
00300 EXPECT_FALSE(cinfo.loadCameraInfo(std::string("package://camera_info_manager/no_such_file.yaml")));
00301 EXPECT_FALSE(cinfo.isCalibrated());
00302 }
00303
00304
00305 TEST(getInfo, invalidLoads)
00306 {
00307 ros::NodeHandle node;
00308 CameraInfoManager cinfo(node);
00309 EXPECT_FALSE(cinfo.isCalibrated());
00310
00311 EXPECT_FALSE(cinfo.loadCameraInfo(std::string("")));
00312 EXPECT_FALSE(cinfo.isCalibrated());
00313
00314 EXPECT_FALSE(cinfo.loadCameraInfo(std::string("flash:///")));
00315 EXPECT_FALSE(cinfo.isCalibrated());
00316
00317 EXPECT_FALSE(cinfo.loadCameraInfo(std::string("html://ros.org/wiki/camera_info_manager")));
00318 EXPECT_FALSE(cinfo.isCalibrated());
00319
00320 sensor_msgs::CameraInfo ci(cinfo.getCameraInfo());
00321 sensor_msgs::CameraInfo exp;
00322 compare_calibration(exp, ci);
00323 }
00324
00325
00326 TEST(setInfo, setCalibration)
00327 {
00328 ros::NodeHandle node;
00329 CameraInfoManager cinfo(node);
00330 EXPECT_FALSE(cinfo.isCalibrated());
00331
00332
00333 sensor_msgs::CameraInfo exp(expected_calibration());
00334 bool success = set_calibration(node, exp);
00335
00336
00337
00338 if (success)
00339 {
00340
00341 EXPECT_TRUE(cinfo.isCalibrated());
00342 sensor_msgs::CameraInfo ci = cinfo.getCameraInfo();
00343 compare_calibration(exp, ci);
00344 }
00345 }
00346
00347
00348 TEST(setInfo, saveCalibrationTmp)
00349 {
00350 ros::NodeHandle node;
00351 sensor_msgs::CameraInfo exp(expected_calibration());
00352 bool success;
00353 std::string tmpFile("/tmp/calibration_camera.yaml");
00354
00355
00356 delete_file(tmpFile);
00357
00358 {
00359
00360 CameraInfoManager cinfo(node);
00361 EXPECT_FALSE(cinfo.isCalibrated());
00362
00363
00364 success = set_calibration(node, exp);
00365 EXPECT_TRUE(cinfo.isCalibrated());
00366 }
00367
00368
00369
00370 if (success)
00371 {
00372
00373 CameraInfoManager cinfo2(node);
00374 EXPECT_FALSE(cinfo2.isCalibrated());
00375 std::string url = "file://" + tmpFile;
00376 cinfo2.loadCameraInfo(url);
00377 EXPECT_TRUE(cinfo2.isCalibrated());
00378 if (cinfo2.isCalibrated())
00379 {
00380 sensor_msgs::CameraInfo ci(cinfo2.getCameraInfo());
00381 compare_calibration(exp, ci);
00382 }
00383 }
00384 }
00385
00386
00387 TEST(setInfo, saveCalibrationCameraName)
00388 {
00389 ros::NodeHandle node;
00390 sensor_msgs::CameraInfo exp(expected_calibration());
00391 std::string tmpFile("/tmp/calibration_" + g_camera_name + ".yaml");
00392 bool success;
00393
00394
00395 delete_file(tmpFile);
00396
00397 {
00398
00399 CameraInfoManager cinfo(node, g_camera_name);
00400 success = set_calibration(node, exp);
00401 EXPECT_TRUE(cinfo.isCalibrated());
00402 }
00403
00404
00405
00406 if (success)
00407 {
00408
00409 CameraInfoManager cinfo2(node);
00410 std::string url = "file://" + tmpFile;
00411 cinfo2.loadCameraInfo(std::string(url));
00412 EXPECT_TRUE(cinfo2.isCalibrated());
00413 if (cinfo2.isCalibrated())
00414 {
00415 sensor_msgs::CameraInfo ci(cinfo2.getCameraInfo());
00416 compare_calibration(exp, ci);
00417 }
00418 }
00419 }
00420
00421
00422 TEST(setInfo, saveCalibrationFile)
00423 {
00424 ros::NodeHandle node;
00425 sensor_msgs::CameraInfo exp(expected_calibration());
00426 std::string cname("camera");
00427 std::string tmpFile("/tmp/camera_info_manager_calibration_test.yaml");
00428 std::string url = "file://" + tmpFile;
00429 bool success;
00430
00431
00432 delete_file(tmpFile);
00433
00434 {
00435
00436 CameraInfoManager cinfo(node, cname, url);
00437 success = set_calibration(node, exp);
00438 EXPECT_TRUE(cinfo.isCalibrated());
00439 }
00440
00441
00442
00443 if (success)
00444 {
00445
00446 CameraInfoManager cinfo2(node, cname, url);
00447 EXPECT_TRUE(cinfo2.isCalibrated());
00448 if (cinfo2.isCalibrated())
00449 {
00450 sensor_msgs::CameraInfo ci(cinfo2.getCameraInfo());
00451 compare_calibration(exp, ci);
00452 }
00453 }
00454 }
00455
00456
00457
00458 TEST(setInfo, saveCalibrationPackage)
00459 {
00460 ros::NodeHandle node;
00461 sensor_msgs::CameraInfo exp(expected_calibration());
00462 std::string pkgPath(ros::package::getPath(g_package_name));
00463 std::string filename(pkgPath + g_package_filename);
00464 bool success;
00465
00466
00467 delete_file(filename);
00468
00469 {
00470
00471 CameraInfoManager cinfo(node, g_camera_name, g_package_url);
00472 success = set_calibration(node, exp);
00473 EXPECT_TRUE(cinfo.isCalibrated());
00474 }
00475
00476
00477
00478 if (success)
00479 {
00480
00481 CameraInfoManager cinfo2(node, g_camera_name, g_package_url);
00482 EXPECT_TRUE(cinfo2.isCalibrated());
00483 if (cinfo2.isCalibrated())
00484 {
00485 sensor_msgs::CameraInfo ci(cinfo2.getCameraInfo());
00486 compare_calibration(exp, ci);
00487 }
00488 }
00489 }
00490
00491
00492 int main(int argc, char **argv)
00493 {
00494 ros::init(argc, argv, "camera_info_manager_unit_test");
00495 testing::InitGoogleTest(&argc, argv);
00496
00497
00498 ros::AsyncSpinner spinner(1);
00499 spinner.start();
00500
00501
00502 return RUN_ALL_TESTS();
00503 }