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 <string>
00038 #include <locale>
00039 #include <stdlib.h>
00040 #include <sys/types.h>
00041 #include <sys/stat.h>
00042 #include <unistd.h>
00043 #include <ros/ros.h>
00044 #include <ros/package.h>
00045 #include <boost/algorithm/string.hpp>
00046 #include <camera_calibration_parsers/parse.h>
00047
00048 #include "camera_info_manager/camera_info_manager.h"
00049
00060 namespace camera_info_manager
00061 {
00062
00063 using namespace camera_calibration_parsers;
00064
00066 const std::string
00067 default_camera_info_url = "file://${ROS_HOME}/camera_info/${NAME}.yaml";
00068
00078 CameraInfoManager::CameraInfoManager(ros::NodeHandle nh,
00079 const std::string &cname,
00080 const std::string &url):
00081 nh_(nh),
00082 camera_name_(cname),
00083 url_(url),
00084 loaded_cam_info_(false)
00085 {
00086
00087 info_service_ = nh_.advertiseService("set_camera_info",
00088 &CameraInfoManager::setCameraInfoService, this);
00089 }
00090
00106 sensor_msgs::CameraInfo CameraInfoManager::getCameraInfo(void)
00107 {
00108 while (ros::ok())
00109 {
00110 std::string cname;
00111 std::string url;
00112 {
00113 boost::mutex::scoped_lock lock_(mutex_);
00114 if (loaded_cam_info_)
00115 {
00116 return cam_info_;
00117 }
00118
00119
00120 loaded_cam_info_ = true;
00121
00122
00123 url = url_;
00124 cname = camera_name_;
00125
00126 }
00127
00128
00129 loadCalibration(url, cname);
00130 }
00131
00132 return sensor_msgs::CameraInfo();
00133 }
00134
00140 std::string CameraInfoManager::getPackageFileName(const std::string &url)
00141 {
00142 ROS_DEBUG_STREAM("camera calibration URL: " << url);
00143
00144
00145
00146 size_t prefix_len = std::string("package://").length();
00147 size_t rest = url.find('/', prefix_len);
00148 std::string package(url.substr(prefix_len, rest - prefix_len));
00149
00150
00151 std::string pkgPath(ros::package::getPath(package));
00152 if (pkgPath.empty())
00153 {
00154 ROS_WARN_STREAM("unknown package: " << package << " (ignored)");
00155 return pkgPath;
00156 }
00157 else
00158 {
00159
00160 return pkgPath + url.substr(rest);
00161 }
00162 }
00163
00173 bool CameraInfoManager::isCalibrated(void)
00174 {
00175 while (true)
00176 {
00177 std::string cname;
00178 std::string url;
00179 {
00180 boost::mutex::scoped_lock lock_(mutex_);
00181 if (loaded_cam_info_)
00182 {
00183 return (cam_info_.K[0] != 0.0);
00184 }
00185
00186
00187 loaded_cam_info_ = true;
00188
00189
00190 url = url_;
00191 cname = camera_name_;
00192
00193 }
00194
00195
00196 loadCalibration(url, cname);
00197 }
00198 }
00199
00210 bool CameraInfoManager::loadCalibration(const std::string &url,
00211 const std::string &cname)
00212 {
00213 bool success = false;
00214
00215 const std::string resURL(resolveURL(url, cname));
00216 url_type_t url_type = parseURL(resURL);
00217
00218 if (url_type != URL_empty)
00219 {
00220 ROS_INFO_STREAM("camera calibration URL: " << resURL);
00221 }
00222
00223 switch (url_type)
00224 {
00225 case URL_empty:
00226 {
00227 ROS_INFO("using default calibration URL");
00228 success = loadCalibration(default_camera_info_url, cname);
00229 break;
00230 }
00231 case URL_file:
00232 {
00233 success = loadCalibrationFile(resURL.substr(7), cname);
00234 break;
00235 }
00236 case URL_flash:
00237 {
00238 ROS_WARN("[CameraInfoManager] reading from flash not implemented yet");
00239 break;
00240 }
00241 case URL_package:
00242 {
00243 std::string filename(getPackageFileName(resURL));
00244 if (!filename.empty())
00245 success = loadCalibrationFile(filename, cname);
00246 break;
00247 }
00248 default:
00249 {
00250 ROS_ERROR_STREAM("Invalid camera calibration URL: " << resURL);
00251 break;
00252 }
00253 }
00254
00255 return success;
00256 }
00257
00268 bool CameraInfoManager::loadCalibrationFile(const std::string &filename,
00269 const std::string &cname)
00270 {
00271 bool success = false;
00272
00273 ROS_DEBUG_STREAM("reading camera calibration from " << filename);
00274 std::string cam_name;
00275 sensor_msgs::CameraInfo cam_info;
00276
00277 if (readCalibration(filename, cam_name, cam_info))
00278 {
00279 if (cname != cam_name)
00280 {
00281 ROS_WARN_STREAM("[" << cname << "] does not match name "
00282 << cam_name << " in file " << filename);
00283 }
00284 success = true;
00285 {
00286
00287 boost::mutex::scoped_lock lock(mutex_);
00288 cam_info_ = cam_info;
00289 }
00290 }
00291 else
00292 {
00293 ROS_WARN_STREAM("Camera calibration file " << filename << " not found.");
00294 }
00295
00296 return success;
00297 }
00298
00310 bool CameraInfoManager::loadCameraInfo(const std::string &url)
00311 {
00312 std::string cname;
00313 {
00314 boost::mutex::scoped_lock lock(mutex_);
00315 url_ = url;
00316 cname = camera_name_;
00317 loaded_cam_info_ = true;
00318 }
00319
00320
00321 return loadCalibration(url, cname);
00322 }
00323
00324
00333 std::string CameraInfoManager::resolveURL(const std::string &url,
00334 const std::string &cname)
00335 {
00336 std::string resolved;
00337 size_t rest = 0;
00338
00339 while (true)
00340 {
00341
00342 size_t dollar = url.find('$', rest);
00343
00344 if (dollar >= url.length())
00345 {
00346
00347 resolved += url.substr(rest);
00348 break;
00349 }
00350
00351
00352 resolved += url.substr(rest, dollar-rest);
00353
00354 if (url.substr(dollar+1, 1) != "{")
00355 {
00356
00357 resolved += "$";
00358 }
00359 else if (url.substr(dollar+1, 6) == "{NAME}")
00360 {
00361
00362 resolved += cname;
00363 dollar += 6;
00364 }
00365 else if (url.substr(dollar+1, 10) == "{ROS_HOME}")
00366 {
00367
00368 std::string ros_home;
00369 char *ros_home_env;
00370 if ((ros_home_env = getenv("ROS_HOME")))
00371 {
00372
00373 ros_home = ros_home_env;
00374 }
00375 else if ((ros_home_env = getenv("HOME")))
00376 {
00377
00378 ros_home = ros_home_env;
00379 ros_home += "/.ros";
00380 }
00381 resolved += ros_home;
00382 dollar += 10;
00383 }
00384 else
00385 {
00386
00387 ROS_ERROR_STREAM("[CameraInfoManager]"
00388 " invalid URL substitution (not resolved): "
00389 << url);
00390 resolved += "$";
00391 }
00392
00393
00394 rest = dollar + 1;
00395 }
00396
00397 return resolved;
00398 }
00399
00407 CameraInfoManager::url_type_t CameraInfoManager::parseURL(const std::string &url)
00408 {
00409 if (url == "")
00410 {
00411 return URL_empty;
00412 }
00413 if (boost::iequals(url.substr(0, 8), "file:///"))
00414 {
00415 return URL_file;
00416 }
00417 if (boost::iequals(url.substr(0, 9), "flash:///"))
00418 {
00419 return URL_flash;
00420 }
00421 if (boost::iequals(url.substr(0, 10), "package://"))
00422 {
00423
00424
00425 size_t rest = url.find('/', 10);
00426 if (rest < url.length()-1 && rest > 10)
00427 return URL_package;
00428 }
00429 return URL_invalid;
00430 }
00431
00442 bool
00443 CameraInfoManager::saveCalibration(const sensor_msgs::CameraInfo &new_info,
00444 const std::string &url,
00445 const std::string &cname)
00446 {
00447 bool success = false;
00448
00449 const std::string resURL(resolveURL(url, cname));
00450
00451 switch (parseURL(resURL))
00452 {
00453 case URL_empty:
00454 {
00455
00456 success = saveCalibration(new_info, default_camera_info_url, cname);
00457 break;
00458 }
00459 case URL_file:
00460 {
00461 success = saveCalibrationFile(new_info, resURL.substr(7), cname);
00462 break;
00463 }
00464 case URL_package:
00465 {
00466 std::string filename(getPackageFileName(resURL));
00467 if (!filename.empty())
00468 success = saveCalibrationFile(new_info, filename, cname);
00469 break;
00470 }
00471 default:
00472 {
00473
00474 ROS_ERROR_STREAM("invalid url: " << resURL << " (ignored)");
00475 success = saveCalibration(new_info, default_camera_info_url, cname);
00476 break;
00477 }
00478 }
00479
00480 return success;
00481 }
00482
00492 bool
00493 CameraInfoManager::saveCalibrationFile(const sensor_msgs::CameraInfo &new_info,
00494 const std::string &filename,
00495 const std::string &cname)
00496 {
00497 ROS_INFO_STREAM("writing calibration data to " << filename);
00498
00499
00500 size_t last_slash = filename.rfind('/');
00501 if (last_slash >= filename.length())
00502 {
00503
00504
00505 ROS_ERROR_STREAM("filename [" << filename << "] has no '/'");
00506 return false;
00507 }
00508
00509
00510 std::string dirname(filename.substr(0, last_slash+1));
00511 struct stat stat_data;
00512 int rc = stat(dirname.c_str(), &stat_data);
00513 if (rc != 0)
00514 {
00515 if (errno == ENOENT)
00516 {
00517
00518 std::string command("mkdir -p " + dirname);
00519 rc = system(command.c_str());
00520 if (rc != 0)
00521 {
00522
00523 ROS_ERROR_STREAM("unable to create path to directory ["
00524 << dirname << "]");
00525 return false;
00526 }
00527 }
00528 else
00529 {
00530
00531 ROS_ERROR_STREAM("directory [" << dirname << "] not accessible");
00532 return false;
00533 }
00534 }
00535 else if (!S_ISDIR(stat_data.st_mode))
00536 {
00537
00538 ROS_ERROR_STREAM("[" << dirname << "] is not a directory");
00539 return false;
00540 }
00541
00542
00543
00544
00545
00546 return writeCalibration(filename, cname, new_info);
00547 }
00548
00557 bool
00558 CameraInfoManager::setCameraInfoService(sensor_msgs::SetCameraInfo::Request &req,
00559 sensor_msgs::SetCameraInfo::Response &rsp)
00560 {
00561
00562 std::string url_copy;
00563 std::string cname;
00564 {
00565 boost::mutex::scoped_lock lock(mutex_);
00566 cam_info_ = req.camera_info;
00567 url_copy = url_;
00568 cname = camera_name_;
00569 loaded_cam_info_ = true;
00570 }
00571
00572 if (!nh_.ok())
00573 {
00574 ROS_ERROR("set_camera_info service called, but driver not running.");
00575 rsp.status_message = "Camera driver not running.";
00576 rsp.success = false;
00577 return false;
00578 }
00579
00580 rsp.success = saveCalibration(req.camera_info, url_copy, cname);
00581 if (!rsp.success)
00582 rsp.status_message = "Error storing camera calibration.";
00583
00584 return true;
00585 }
00586
00597 bool CameraInfoManager::setCameraName(const std::string &cname)
00598 {
00599
00600 if (cname.empty())
00601 return false;
00602
00603
00604 for (unsigned i = 0; i < cname.size(); ++i)
00605 {
00606 if (!isalnum(cname[i]) && cname[i] != '_')
00607 return false;
00608 }
00609
00610
00611
00612
00613 {
00614 boost::mutex::scoped_lock lock(mutex_);
00615 camera_name_ = cname;
00616 loaded_cam_info_ = false;
00617 }
00618
00619 return true;
00620 }
00621
00630 bool CameraInfoManager::setCameraInfo(const sensor_msgs::CameraInfo &camera_info)
00631 {
00632 boost::mutex::scoped_lock lock(mutex_);
00633
00634 cam_info_ = camera_info;
00635 loaded_cam_info_ = true;
00636
00637 return true;
00638 }
00639
00647 bool CameraInfoManager::validateURL(const std::string &url)
00648 {
00649 std::string cname;
00650 {
00651 boost::mutex::scoped_lock lock(mutex_);
00652 cname = camera_name_;
00653 }
00654
00655 url_type_t url_type = parseURL(resolveURL(url, cname));
00656 return (url_type < URL_invalid);
00657 }
00658
00659 }