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 <ros/ros.h>
00040 #include <ros/package.h>
00041 #include <camera_calibration_parsers/parse.h>
00042
00043 #include "camera_info_manager/camera_info_manager.h"
00044
00045 using namespace camera_calibration_parsers;
00046
00067 CameraInfoManager::CameraInfoManager(ros::NodeHandle nh,
00068 const std::string &cname,
00069 const std::string &url):
00070 nh_(nh),
00071 camera_name_(cname)
00072 {
00073
00074 loadCameraInfo(url);
00075
00076
00077 info_service_ = nh_.advertiseService("set_camera_info",
00078 &CameraInfoManager::setCameraInfo, this);
00079 }
00080
00086 std::string CameraInfoManager::getPackageFileName(const std::string &url)
00087 {
00088 ROS_DEBUG_STREAM("camera calibration URL: " << url);
00089
00090
00091
00092 size_t prefix_len = std::string("package://").length();
00093 size_t rest = url.find('/', prefix_len);
00094 std::string package(url.substr(prefix_len, rest - prefix_len));
00095
00096
00097 std::string pkgPath(ros::package::getPath(package));
00098 if (pkgPath.empty())
00099 {
00100 ROS_WARN_STREAM("unknown package: " << package << " (ignored)");
00101 return pkgPath;
00102 }
00103 else
00104 {
00105
00106 return pkgPath + url.substr(rest);
00107 }
00108 }
00109
00120 bool CameraInfoManager::loadCalibration(const std::string &url,
00121 const std::string &cname)
00122 {
00123 bool success = false;
00124
00125 url_type_t url_type = parseURL(url);
00126
00127 if (url_type != URL_empty)
00128 {
00129 ROS_INFO_STREAM("camera calibration URL: " << url);
00130 }
00131
00132 switch (url_type)
00133 {
00134 case URL_empty:
00135 {
00136 ROS_DEBUG("no camera calibration source");
00137 break;
00138 }
00139 case URL_file:
00140 {
00141 success = loadCalibrationFile(url.substr(7), cname);
00142 break;
00143 }
00144 case URL_flash:
00145 {
00146 ROS_WARN("[CameraInfoManager] reading from flash not implemented yet");
00147 break;
00148 }
00149 case URL_package:
00150 {
00151 std::string filename(getPackageFileName(url));
00152 if (!filename.empty())
00153 success = loadCalibrationFile(filename, cname);
00154 break;
00155 }
00156 default:
00157 {
00158 ROS_ERROR_STREAM("Invalid camera calibration URL: " << url);
00159 break;
00160 }
00161 }
00162
00163 return success;
00164 }
00165
00176 bool CameraInfoManager::loadCalibrationFile(const std::string &filename,
00177 const std::string &cname)
00178 {
00179 bool success = false;
00180
00181 ROS_DEBUG_STREAM("reading camera calibration from " << filename);
00182 std::string cam_name;
00183 sensor_msgs::CameraInfo cam_info;
00184
00185 if (readCalibration(filename, cam_name, cam_info))
00186 {
00187 if (cname != cam_name)
00188 {
00189 ROS_WARN_STREAM("[" << cname << "] does not match name "
00190 << cam_name << " in file " << filename);
00191 }
00192 success = true;
00193 {
00194
00195 boost::mutex::scoped_lock lock(mutex_);
00196 cam_info_ = cam_info;
00197 }
00198 }
00199 else
00200 {
00201 ROS_WARN_STREAM("Camera calibration file " << filename << " not found.");
00202 }
00203
00204 return success;
00205 }
00206
00214 bool CameraInfoManager::loadCameraInfo(const std::string &url)
00215 {
00216 std::string cname;
00217 {
00218 boost::mutex::scoped_lock lock(mutex_);
00219 url_ = url;
00220 cname = camera_name_;
00221 }
00222
00223
00224 return loadCalibration(url, cname);
00225 }
00226
00227
00235 CameraInfoManager::url_type_t CameraInfoManager::parseURL(const std::string &url)
00236 {
00237 if (url == "")
00238 {
00239 return URL_empty;
00240 }
00241 if (url.substr(0, 8) == "file:///")
00242 {
00243 return URL_file;
00244 }
00245 if (url.substr(0, 9) == "flash:///")
00246 {
00247 return URL_flash;
00248 }
00249 if (url.substr(0, 10) == "package://")
00250 {
00251
00252
00253 size_t rest = url.find('/', 10);
00254 if (rest < url.length()-1 && rest > 10)
00255 return URL_package;
00256 }
00257 return URL_invalid;
00258 }
00259
00270 bool
00271 CameraInfoManager::saveCalibration(const sensor_msgs::CameraInfo &new_info,
00272 const std::string &url,
00273 const std::string &cname)
00274 {
00275 bool success = false;
00276
00277 switch (parseURL(url))
00278 {
00279 case URL_empty:
00280 {
00281
00282 std::string filename("/tmp/calibration_" + cname + ".yaml");
00283 success = saveCalibrationFile(new_info, filename, cname);
00284 break;
00285 }
00286 case URL_file:
00287 {
00288 success = saveCalibrationFile(new_info, url.substr(7), cname);
00289 break;
00290 }
00291 case URL_package:
00292 {
00293 std::string filename(getPackageFileName(url));
00294 if (!filename.empty())
00295 success = saveCalibrationFile(new_info, filename, cname);
00296 break;
00297 }
00298 default:
00299 {
00300
00301 ROS_ERROR_STREAM("invalid url: " << url << " (ignored)");
00302 success = saveCalibration(new_info, std::string(""), cname);
00303 break;
00304 }
00305 }
00306
00307 return success;
00308 }
00309
00319 bool
00320 CameraInfoManager::saveCalibrationFile(const sensor_msgs::CameraInfo &new_info,
00321 const std::string &filename,
00322 const std::string &cname)
00323 {
00324 ROS_INFO_STREAM("writing calibration data to " << filename);
00325 return writeCalibration(filename, cname, new_info);
00326 }
00327
00336 bool
00337 CameraInfoManager::setCameraInfo(sensor_msgs::SetCameraInfo::Request &req,
00338 sensor_msgs::SetCameraInfo::Response &rsp)
00339 {
00340
00341 std::string url_copy;
00342 std::string cname;
00343 {
00344 boost::mutex::scoped_lock lock(mutex_);
00345 cam_info_ = req.camera_info;
00346 url_copy = url_;
00347 cname = camera_name_;
00348 }
00349
00350 if (!nh_.ok())
00351 {
00352 ROS_ERROR("set_camera_info service called, but driver not running.");
00353 rsp.status_message = "Camera driver not running.";
00354 rsp.success = false;
00355 return false;
00356 }
00357
00358 rsp.success = saveCalibration(req.camera_info, url_copy, cname);
00359 if (!rsp.success)
00360 rsp.status_message = "Error storing camera calibration.";
00361
00362 return true;
00363 }
00364
00374 bool CameraInfoManager::setCameraName(const std::string &cname)
00375 {
00376
00377 if (cname.empty())
00378 return false;
00379
00380
00381 for (unsigned i = 0; i < cname.size(); ++i)
00382 {
00383 if (!isalnum(cname[i]) && cname[i] != '_')
00384 return false;
00385 }
00386
00387
00388 boost::mutex::scoped_lock lock(mutex_);
00389 camera_name_ = cname;
00390 return true;
00391 }
00392
00400 bool CameraInfoManager::validateURL(const std::string &url)
00401 {
00402 url_type_t url_type = parseURL(url);
00403 return (url_type < URL_invalid);
00404 }