camera_info_manager.cpp
Go to the documentation of this file.
00001 /* $Id$ */
00002 
00003 /*********************************************************************
00004 * Software License Agreement (BSD License)
00005 *
00006 *  Copyright (c) 2010-2012 Jack O'Quin
00007 *  All rights reserved.
00008 *
00009 *  Redistribution and use in source and binary forms, with or without
00010 *  modification, are permitted provided that the following conditions
00011 *  are met:
00012 *
00013 *   * Redistributions of source code must retain the above copyright
00014 *     notice, this list of conditions and the following disclaimer.
00015 *   * Redistributions in binary form must reproduce the above
00016 *     copyright notice, this list of conditions and the following
00017 *     disclaimer in the documentation and/or other materials provided
00018 *     with the distribution.
00019 *   * Neither the name of the author nor other contributors may be
00020 *     used to endorse or promote products derived from this software
00021 *     without specific prior written permission.
00022 *
00023 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 *  POSSIBILITY OF SUCH DAMAGE.
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   // register callback for camera calibration service request
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_;           // all done
00117           }
00118 
00119         // load being attempted now
00120         loaded_cam_info_ = true;
00121 
00122         // copy the name and URL strings
00123         url = url_;
00124         cname = camera_name_;
00125 
00126       } // release the lock
00127 
00128       // attempt load without the lock, it is not recursive
00129       loadCalibration(url, cname);
00130     }
00131 }
00132 
00138 std::string CameraInfoManager::getPackageFileName(const std::string &url)
00139 {
00140   ROS_DEBUG_STREAM("camera calibration URL: " << url);
00141 
00142   // Scan URL from after "package://" until next '/' and extract
00143   // package name.  The parseURL() already checked that it's present.
00144   size_t prefix_len = std::string("package://").length();
00145   size_t rest = url.find('/', prefix_len);
00146   std::string package(url.substr(prefix_len, rest - prefix_len));
00147 
00148   // Look up the ROS package path name.
00149   std::string pkgPath(ros::package::getPath(package));
00150   if (pkgPath.empty())                  // package not found?
00151     {
00152       ROS_WARN_STREAM("unknown package: " << package << " (ignored)");
00153       return pkgPath;
00154     }
00155   else
00156     {
00157       // Construct file name from package location and remainder of URL.
00158       return pkgPath + url.substr(rest);
00159     }
00160 }
00161 
00171 bool CameraInfoManager::isCalibrated(void)
00172 {
00173   while (true)
00174     {
00175       std::string cname;
00176       std::string url;
00177       {
00178         boost::mutex::scoped_lock lock_(mutex_);
00179         if (loaded_cam_info_)
00180           {
00181             return (cam_info_.K[0] != 0.0);
00182           }
00183 
00184         // load being attempted now
00185         loaded_cam_info_ = true;
00186 
00187         // copy the name and URL strings
00188         url = url_;
00189         cname = camera_name_;
00190 
00191       } // release the lock
00192 
00193       // attempt load without the lock, it is not recursive
00194       loadCalibration(url, cname);
00195     }
00196 }
00197 
00208 bool CameraInfoManager::loadCalibration(const std::string &url,
00209                                         const std::string &cname)
00210 {
00211   bool success = false;                 // return value
00212 
00213   const std::string resURL(resolveURL(url, cname));
00214   url_type_t url_type = parseURL(resURL);
00215 
00216   if (url_type != URL_empty)
00217     {
00218       ROS_INFO_STREAM("camera calibration URL: " << resURL);
00219     }
00220 
00221   switch (url_type)
00222     {
00223     case URL_empty:
00224       {
00225         ROS_INFO("using default calibration URL");
00226         success = loadCalibration(default_camera_info_url, cname);
00227         break;
00228       }
00229     case URL_file:
00230       {
00231         success = loadCalibrationFile(resURL.substr(7), cname);
00232         break;
00233       }
00234     case URL_flash:
00235       {
00236         ROS_WARN("[CameraInfoManager] reading from flash not implemented yet");
00237         break;
00238       }
00239     case URL_package:
00240       {
00241         std::string filename(getPackageFileName(resURL));
00242         if (!filename.empty())
00243           success = loadCalibrationFile(filename, cname);
00244         break;
00245       }
00246     default:
00247       {
00248         ROS_ERROR_STREAM("Invalid camera calibration URL: " << resURL);
00249         break;
00250       }
00251     }
00252 
00253   return success;
00254 }
00255 
00266 bool CameraInfoManager::loadCalibrationFile(const std::string &filename,
00267                                             const std::string &cname)
00268 {
00269   bool success = false;
00270 
00271   ROS_DEBUG_STREAM("reading camera calibration from " << filename);
00272   std::string cam_name;
00273   sensor_msgs::CameraInfo cam_info;
00274 
00275   if (readCalibration(filename, cam_name, cam_info))
00276     {
00277       if (cname != cam_name)
00278         {
00279           ROS_WARN_STREAM("[" << cname << "] does not match name "
00280                           << cam_name << " in file " << filename);
00281         }
00282       success = true;
00283       {
00284         // lock only while updating cam_info_
00285         boost::mutex::scoped_lock lock(mutex_);
00286         cam_info_ = cam_info;
00287       }
00288     }
00289   else
00290     {
00291       ROS_WARN_STREAM("Camera calibration file " << filename << " not found.");
00292     }
00293 
00294   return success;
00295 }
00296 
00308 bool CameraInfoManager::loadCameraInfo(const std::string &url)
00309 {
00310   std::string cname;
00311   {
00312     boost::mutex::scoped_lock lock(mutex_);
00313     url_ = url;
00314     cname = camera_name_;
00315     loaded_cam_info_ = true;
00316   }
00317 
00318   // load using copies of the parameters, no need to hold the lock
00319   return loadCalibration(url, cname);
00320 }
00321 
00322 
00331 std::string CameraInfoManager::resolveURL(const std::string &url,
00332                                           const std::string &cname)
00333 {
00334   std::string resolved;
00335   size_t rest = 0;
00336 
00337   while (true)
00338     {
00339       // find the next '$' in the URL string
00340       size_t dollar  = url.find('$', rest);
00341 
00342       if (dollar >= url.length())
00343         {
00344           // no more variables left in the URL
00345           resolved += url.substr(rest);
00346           break;
00347         }
00348 
00349       // copy characters up to the next '$'
00350       resolved += url.substr(rest, dollar-rest);
00351 
00352       if (url.substr(dollar+1, 1) != "{")
00353         {
00354           // no '{' follows, so keep the '$'
00355           resolved += "$";
00356         }
00357       else if (url.substr(dollar+1, 6) == "{NAME}")
00358         {
00359           // substitute camera name
00360           resolved += cname;
00361           dollar += 6;
00362         }
00363       else if (url.substr(dollar+1, 10) == "{ROS_HOME}")
00364         {
00365           // substitute $ROS_HOME
00366           std::string ros_home;
00367           char *ros_home_env;
00368           if ((ros_home_env = getenv("ROS_HOME")))
00369             {
00370               // use environment variable
00371               ros_home = ros_home_env;
00372             }
00373           else if ((ros_home_env = getenv("HOME")))
00374             {
00375               // use "$HOME/.ros"
00376               ros_home = ros_home_env;
00377               ros_home += "/.ros";
00378             }
00379           resolved += ros_home;
00380           dollar += 10;
00381         }
00382       else
00383         {
00384           // not a valid substitution variable
00385           ROS_ERROR_STREAM("[CameraInfoManager]"
00386                            " invalid URL substitution (not resolved): "
00387                            << url);
00388           resolved += "$";            // keep the bogus '$'
00389         }
00390 
00391       // look for next '$'
00392       rest = dollar + 1;
00393     }
00394 
00395   return resolved;
00396 }
00397 
00405 CameraInfoManager::url_type_t CameraInfoManager::parseURL(const std::string &url)
00406 {
00407   if (url == "")
00408     {
00409       return URL_empty;
00410     }
00411   if (boost::iequals(url.substr(0, 8), "file:///"))
00412     {
00413       return URL_file;
00414     }
00415   if (boost::iequals(url.substr(0, 9), "flash:///"))
00416     {
00417       return URL_flash;
00418     }
00419   if (boost::iequals(url.substr(0, 10), "package://"))
00420     {
00421       // look for a '/' following the package name, make sure it is
00422       // there, the name is not empty, and something follows it
00423       size_t rest = url.find('/', 10);
00424       if (rest < url.length()-1 && rest > 10)
00425         return URL_package;
00426     }
00427   return URL_invalid;
00428 }
00429 
00440 bool
00441 CameraInfoManager::saveCalibration(const sensor_msgs::CameraInfo &new_info,
00442                                    const std::string &url,
00443                                    const std::string &cname)
00444 {
00445   bool success = false;
00446 
00447   const std::string resURL(resolveURL(url, cname));
00448 
00449   switch (parseURL(resURL))
00450     {
00451     case URL_empty:
00452       {
00453         // store using default file name
00454         success = saveCalibration(new_info, default_camera_info_url, cname);
00455         break;
00456       }
00457     case URL_file:
00458       {
00459         success = saveCalibrationFile(new_info, resURL.substr(7), cname);
00460         break;
00461       }
00462     case URL_package:
00463       {
00464         std::string filename(getPackageFileName(resURL));
00465         if (!filename.empty())
00466           success = saveCalibrationFile(new_info, filename, cname);
00467         break;
00468       }
00469     default:
00470       {
00471         // invalid URL, save to default location
00472         ROS_ERROR_STREAM("invalid url: " << resURL << " (ignored)");
00473         success = saveCalibration(new_info, default_camera_info_url, cname);
00474         break;
00475       }
00476     }
00477 
00478   return success;
00479 }
00480   
00490 bool
00491 CameraInfoManager::saveCalibrationFile(const sensor_msgs::CameraInfo &new_info,
00492                                        const std::string &filename,
00493                                        const std::string &cname)
00494 {
00495   ROS_INFO_STREAM("writing calibration data to " << filename);
00496 
00497   // isolate the name of the containing directory
00498   size_t last_slash = filename.rfind('/');
00499   if (last_slash >= filename.length())
00500     {
00501       // No slash in the name.  This should never happen, the URL
00502       // parser ensures there is at least one '/' at the beginning.
00503       ROS_ERROR_STREAM("filename [" << filename << "] has no '/'");
00504       return false;                     // not a valid URL
00505     }
00506 
00507   // make sure the directory exists and is writable
00508   std::string dirname(filename.substr(0, last_slash+1));
00509   struct stat stat_data;
00510   int rc = stat(dirname.c_str(), &stat_data);
00511   if (rc != 0)
00512     {
00513       if (errno == ENOENT)
00514         {
00515           // directory does not exist, try to create it and its parents
00516           std::string command("mkdir -p " + dirname);
00517           rc = system(command.c_str());
00518           if (rc != 0)
00519             {
00520               // mkdir failed
00521               ROS_ERROR_STREAM("unable to create path to directory ["
00522                                << dirname << "]");
00523               return false;
00524             }
00525         }
00526       else
00527         {
00528           // not accessible, or something screwy
00529           ROS_ERROR_STREAM("directory [" << dirname << "] not accessible");
00530           return false;
00531         }
00532     }
00533   else if (!S_ISDIR(stat_data.st_mode))
00534     {
00535       // dirname exists but is not a directory
00536       ROS_ERROR_STREAM("[" << dirname << "] is not a directory");
00537       return false;
00538     }
00539 
00540   // Directory exists and is accessible. Permissions might still be bad.
00541 
00542   // Currently, writeCalibration() always returns true no matter what
00543   // (ros-pkg ticket #5010).
00544   return writeCalibration(filename, cname, new_info);
00545 }
00546 
00555 bool 
00556 CameraInfoManager::setCameraInfoService(sensor_msgs::SetCameraInfo::Request &req,
00557                                         sensor_msgs::SetCameraInfo::Response &rsp)
00558 {
00559   // copies of class variables needed for saving calibration
00560   std::string url_copy;
00561   std::string cname;
00562   {
00563     boost::mutex::scoped_lock lock(mutex_);
00564     cam_info_ = req.camera_info;
00565     url_copy = url_;
00566     cname = camera_name_;
00567     loaded_cam_info_ = true;
00568   }
00569 
00570   if (!nh_.ok())
00571     {
00572       ROS_ERROR("set_camera_info service called, but driver not running.");
00573       rsp.status_message = "Camera driver not running.";
00574       rsp.success = false;
00575       return false;
00576     }
00577 
00578   rsp.success = saveCalibration(req.camera_info, url_copy, cname);
00579   if (!rsp.success)
00580     rsp.status_message = "Error storing camera calibration.";
00581 
00582   return true;
00583 }
00584 
00595 bool CameraInfoManager::setCameraName(const std::string &cname)
00596 {
00597   // the camera name may not be empty
00598   if (cname.empty())
00599     return false;
00600 
00601   // validate the camera name characters
00602   for (unsigned i = 0; i < cname.size(); ++i)
00603     {
00604       if (!isalnum(cname[i]) && cname[i] != '_')
00605         return false;
00606     }
00607 
00608   // The name is valid, so update our private copy.  Since the new
00609   // name might cause the existing URL to resolve somewhere else,
00610   // force @c cam_info_ to be reloaded before being used again.
00611   {
00612     boost::mutex::scoped_lock lock(mutex_);
00613     camera_name_ = cname;
00614     loaded_cam_info_ = false;
00615   }
00616 
00617   return true;
00618 }
00619 
00628 bool CameraInfoManager::setCameraInfo(const sensor_msgs::CameraInfo &camera_info)
00629 {
00630   boost::mutex::scoped_lock lock(mutex_);
00631 
00632   cam_info_ = camera_info;
00633   loaded_cam_info_ = true;
00634 
00635   return true;
00636 }
00637 
00645 bool CameraInfoManager::validateURL(const std::string &url)
00646 {
00647   std::string cname;                    // copy of camera name
00648   {
00649     boost::mutex::scoped_lock lock(mutex_);
00650     cname = camera_name_;
00651   } // release the lock
00652 
00653   url_type_t url_type = parseURL(resolveURL(url, cname));
00654   return (url_type < URL_invalid);
00655 }
00656 
00657 } // namespace camera_info_manager


camera_info_manager
Author(s): Jack O'Quin
autogenerated on Thu Aug 27 2015 13:30:39