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   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   // Scan URL from after "package://" until next '/' and extract
00145   // package name.  The parseURL() already checked that it's present.
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   // Look up the ROS package path name.
00151   std::string pkgPath(ros::package::getPath(package));
00152   if (pkgPath.empty())                  // package not found?
00153     {
00154       ROS_WARN_STREAM("unknown package: " << package << " (ignored)");
00155       return pkgPath;
00156     }
00157   else
00158     {
00159       // Construct file name from package location and remainder of URL.
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         // load being attempted now
00187         loaded_cam_info_ = true;
00188 
00189         // copy the name and URL strings
00190         url = url_;
00191         cname = camera_name_;
00192 
00193       } // release the lock
00194 
00195       // attempt load without the lock, it is not recursive
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;                 // return value
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         // lock only while updating cam_info_
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   // load using copies of the parameters, no need to hold the lock
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       // find the next '$' in the URL string
00342       size_t dollar  = url.find('$', rest);
00343 
00344       if (dollar >= url.length())
00345         {
00346           // no more variables left in the URL
00347           resolved += url.substr(rest);
00348           break;
00349         }
00350 
00351       // copy characters up to the next '$'
00352       resolved += url.substr(rest, dollar-rest);
00353 
00354       if (url.substr(dollar+1, 1) != "{")
00355         {
00356           // no '{' follows, so keep the '$'
00357           resolved += "$";
00358         }
00359       else if (url.substr(dollar+1, 6) == "{NAME}")
00360         {
00361           // substitute camera name
00362           resolved += cname;
00363           dollar += 6;
00364         }
00365       else if (url.substr(dollar+1, 10) == "{ROS_HOME}")
00366         {
00367           // substitute $ROS_HOME
00368           std::string ros_home;
00369           char *ros_home_env;
00370           if ((ros_home_env = getenv("ROS_HOME")))
00371             {
00372               // use environment variable
00373               ros_home = ros_home_env;
00374             }
00375           else if ((ros_home_env = getenv("HOME")))
00376             {
00377               // use "$HOME/.ros"
00378               ros_home = ros_home_env;
00379               ros_home += "/.ros";
00380             }
00381           resolved += ros_home;
00382           dollar += 10;
00383         }
00384       else
00385         {
00386           // not a valid substitution variable
00387           ROS_ERROR_STREAM("[CameraInfoManager]"
00388                            " invalid URL substitution (not resolved): "
00389                            << url);
00390           resolved += "$";            // keep the bogus '$'
00391         }
00392 
00393       // look for next '$'
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       // look for a '/' following the package name, make sure it is
00424       // there, the name is not empty, and something follows it
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         // store using default file name
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         // invalid URL, save to default location
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   // isolate the name of the containing directory
00500   size_t last_slash = filename.rfind('/');
00501   if (last_slash >= filename.length())
00502     {
00503       // No slash in the name.  This should never happen, the URL
00504       // parser ensures there is at least one '/' at the beginning.
00505       ROS_ERROR_STREAM("filename [" << filename << "] has no '/'");
00506       return false;                     // not a valid URL
00507     }
00508 
00509   // make sure the directory exists and is writable
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           // directory does not exist, try to create it and its parents
00518           std::string command("mkdir -p " + dirname);
00519           rc = system(command.c_str());
00520           if (rc != 0)
00521             {
00522               // mkdir failed
00523               ROS_ERROR_STREAM("unable to create path to directory ["
00524                                << dirname << "]");
00525               return false;
00526             }
00527         }
00528       else
00529         {
00530           // not accessible, or something screwy
00531           ROS_ERROR_STREAM("directory [" << dirname << "] not accessible");
00532           return false;
00533         }
00534     }
00535   else if (!S_ISDIR(stat_data.st_mode))
00536     {
00537       // dirname exists but is not a directory
00538       ROS_ERROR_STREAM("[" << dirname << "] is not a directory");
00539       return false;
00540     }
00541 
00542   // Directory exists and is accessible. Permissions might still be bad.
00543 
00544   // Currently, writeCalibration() always returns true no matter what
00545   // (ros-pkg ticket #5010).
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   // copies of class variables needed for saving calibration
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   // the camera name may not be empty
00600   if (cname.empty())
00601     return false;
00602 
00603   // validate the camera name characters
00604   for (unsigned i = 0; i < cname.size(); ++i)
00605     {
00606       if (!isalnum(cname[i]) && cname[i] != '_')
00607         return false;
00608     }
00609 
00610   // The name is valid, so update our private copy.  Since the new
00611   // name might cause the existing URL to resolve somewhere else,
00612   // force @c cam_info_ to be reloaded before being used again.
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;                    // copy of camera name
00650   {
00651     boost::mutex::scoped_lock lock(mutex_);
00652     cname = camera_name_;
00653   } // release the lock
00654 
00655   url_type_t url_type = parseURL(resolveURL(url, cname));
00656   return (url_type < URL_invalid);
00657 }
00658 
00659 } // namespace camera_info_manager


camera_info_manager
Author(s): Jack O'Quin
autogenerated on Thu Jun 6 2019 21:20:02