camera_info_manager.cpp
Go to the documentation of this file.
1 /* $Id$ */
2 
3 /*********************************************************************
4 * Software License Agreement (BSD License)
5 *
6 * Copyright (c) 2010-2012 Jack O'Quin
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the author nor other contributors may be
20 * used to endorse or promote products derived from this software
21 * without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *********************************************************************/
36 
37 #include <string>
38 #include <locale>
39 #include <stdlib.h>
40 #include <sys/types.h>
41 #include <sys/stat.h>
42 #ifdef _WIN32
43  #ifndef S_ISDIR
44  #define S_ISDIR(mode) (((mode) & S_IFMT) == S_IFDIR)
45  #endif
46 #else
47  #include <unistd.h>
48 #endif
49 
50 #include <ros/ros.h>
51 #include <ros/package.h>
52 #include <boost/algorithm/string.hpp>
54 
56 
67 namespace camera_info_manager
68 {
69 
70 using namespace camera_calibration_parsers;
71 
73 const std::string
74  default_camera_info_url = "file://${ROS_HOME}/camera_info/${NAME}.yaml";
75 
86  const std::string &cname,
87  const std::string &url):
88  nh_(nh),
89  camera_name_(cname),
90  url_(url),
91  loaded_cam_info_(false)
92 {
93  // register callback for camera calibration service request
94  info_service_ = nh_.advertiseService("set_camera_info",
96 }
97 
113 sensor_msgs::CameraInfo CameraInfoManager::getCameraInfo(void)
114 {
115  while (ros::ok())
116  {
117  std::string cname;
118  std::string url;
119  {
120  boost::mutex::scoped_lock lock_(mutex_);
121  if (loaded_cam_info_)
122  {
123  return cam_info_; // all done
124  }
125 
126  // load being attempted now
127  loaded_cam_info_ = true;
128 
129  // copy the name and URL strings
130  url = url_;
131  cname = camera_name_;
132 
133  } // release the lock
134 
135  // attempt load without the lock, it is not recursive
136  loadCalibration(url, cname);
137  }
138 
139  return sensor_msgs::CameraInfo();
140 }
141 
147 std::string CameraInfoManager::getPackageFileName(const std::string &url)
148 {
149  ROS_DEBUG_STREAM("camera calibration URL: " << url);
150 
151  // Scan URL from after "package://" until next '/' and extract
152  // package name. The parseURL() already checked that it's present.
153  size_t prefix_len = std::string("package://").length();
154  size_t rest = url.find('/', prefix_len);
155  std::string package(url.substr(prefix_len, rest - prefix_len));
156 
157  // Look up the ROS package path name.
158  std::string pkgPath(ros::package::getPath(package));
159  if (pkgPath.empty()) // package not found?
160  {
161  ROS_WARN_STREAM("unknown package: " << package << " (ignored)");
162  return pkgPath;
163  }
164  else
165  {
166  // Construct file name from package location and remainder of URL.
167  return pkgPath + url.substr(rest);
168  }
169 }
170 
181 {
182  while (true)
183  {
184  std::string cname;
185  std::string url;
186  {
187  boost::mutex::scoped_lock lock_(mutex_);
188  if (loaded_cam_info_)
189  {
190  return (cam_info_.K[0] != 0.0);
191  }
192 
193  // load being attempted now
194  loaded_cam_info_ = true;
195 
196  // copy the name and URL strings
197  url = url_;
198  cname = camera_name_;
199 
200  } // release the lock
201 
202  // attempt load without the lock, it is not recursive
203  loadCalibration(url, cname);
204  }
205 }
206 
217 bool CameraInfoManager::loadCalibration(const std::string &url,
218  const std::string &cname)
219 {
220  bool success = false; // return value
221 
222  const std::string resURL(resolveURL(url, cname));
223  url_type_t url_type = parseURL(resURL);
224 
225  if (url_type != URL_empty)
226  {
227  ROS_INFO_STREAM("camera calibration URL: " << resURL);
228  }
229 
230  switch (url_type)
231  {
232  case URL_empty:
233  {
234  ROS_INFO("using default calibration URL");
235  success = loadCalibration(default_camera_info_url, cname);
236  break;
237  }
238  case URL_file:
239  {
240  success = loadCalibrationFile(resURL.substr(7), cname);
241  break;
242  }
243  case URL_flash:
244  {
245  ROS_WARN("[CameraInfoManager] reading from flash not implemented yet");
246  break;
247  }
248  case URL_package:
249  {
250  std::string filename(getPackageFileName(resURL));
251  if (!filename.empty())
252  success = loadCalibrationFile(filename, cname);
253  break;
254  }
255  default:
256  {
257  ROS_ERROR_STREAM("Invalid camera calibration URL: " << resURL);
258  break;
259  }
260  }
261 
262  return success;
263 }
264 
275 bool CameraInfoManager::loadCalibrationFile(const std::string &filename,
276  const std::string &cname)
277 {
278  bool success = false;
279 
280  ROS_DEBUG_STREAM("reading camera calibration from " << filename);
281  std::string cam_name;
282  sensor_msgs::CameraInfo cam_info;
283 
284  if (readCalibration(filename, cam_name, cam_info))
285  {
286  if (cname != cam_name)
287  {
288  ROS_WARN_STREAM("[" << cname << "] does not match name "
289  << cam_name << " in file " << filename);
290  }
291  success = true;
292  {
293  // lock only while updating cam_info_
294  boost::mutex::scoped_lock lock(mutex_);
295  cam_info_ = cam_info;
296  }
297  }
298  else
299  {
300  ROS_WARN_STREAM("Camera calibration file " << filename << " not found.");
301  }
302 
303  return success;
304 }
305 
317 bool CameraInfoManager::loadCameraInfo(const std::string &url)
318 {
319  std::string cname;
320  {
321  boost::mutex::scoped_lock lock(mutex_);
322  url_ = url;
323  cname = camera_name_;
324  loaded_cam_info_ = true;
325  }
326 
327  // load using copies of the parameters, no need to hold the lock
328  return loadCalibration(url, cname);
329 }
330 
331 
340 std::string CameraInfoManager::resolveURL(const std::string &url,
341  const std::string &cname)
342 {
343  std::string resolved;
344  size_t rest = 0;
345 
346  while (true)
347  {
348  // find the next '$' in the URL string
349  size_t dollar = url.find('$', rest);
350 
351  if (dollar >= url.length())
352  {
353  // no more variables left in the URL
354  resolved += url.substr(rest);
355  break;
356  }
357 
358  // copy characters up to the next '$'
359  resolved += url.substr(rest, dollar-rest);
360 
361  if (url.substr(dollar+1, 1) != "{")
362  {
363  // no '{' follows, so keep the '$'
364  resolved += "$";
365  }
366  else if (url.substr(dollar+1, 6) == "{NAME}")
367  {
368  // substitute camera name
369  resolved += cname;
370  dollar += 6;
371  }
372  else if (url.substr(dollar+1, 10) == "{ROS_HOME}")
373  {
374  // substitute $ROS_HOME
375  std::string ros_home;
376  char *ros_home_env;
377  if ((ros_home_env = getenv("ROS_HOME")))
378  {
379  // use environment variable
380  ros_home = ros_home_env;
381  }
382  else if ((ros_home_env = getenv("HOME")))
383  {
384  // use "$HOME/.ros"
385  ros_home = ros_home_env;
386  ros_home += "/.ros";
387  }
388  resolved += ros_home;
389  dollar += 10;
390  }
391  else
392  {
393  // not a valid substitution variable
394  ROS_ERROR_STREAM("[CameraInfoManager]"
395  " invalid URL substitution (not resolved): "
396  << url);
397  resolved += "$"; // keep the bogus '$'
398  }
399 
400  // look for next '$'
401  rest = dollar + 1;
402  }
403 
404  return resolved;
405 }
406 
415 {
416  if (url == "")
417  {
418  return URL_empty;
419  }
420  if (boost::iequals(url.substr(0, 8), "file:///"))
421  {
422  return URL_file;
423  }
424  if (boost::iequals(url.substr(0, 9), "flash:///"))
425  {
426  return URL_flash;
427  }
428  if (boost::iequals(url.substr(0, 10), "package://"))
429  {
430  // look for a '/' following the package name, make sure it is
431  // there, the name is not empty, and something follows it
432  size_t rest = url.find('/', 10);
433  if (rest < url.length()-1 && rest > 10)
434  return URL_package;
435  }
436  return URL_invalid;
437 }
438 
449 bool
450 CameraInfoManager::saveCalibration(const sensor_msgs::CameraInfo &new_info,
451  const std::string &url,
452  const std::string &cname)
453 {
454  bool success = false;
455 
456  const std::string resURL(resolveURL(url, cname));
457 
458  switch (parseURL(resURL))
459  {
460  case URL_empty:
461  {
462  // store using default file name
463  success = saveCalibration(new_info, default_camera_info_url, cname);
464  break;
465  }
466  case URL_file:
467  {
468  success = saveCalibrationFile(new_info, resURL.substr(7), cname);
469  break;
470  }
471  case URL_package:
472  {
473  std::string filename(getPackageFileName(resURL));
474  if (!filename.empty())
475  success = saveCalibrationFile(new_info, filename, cname);
476  break;
477  }
478  default:
479  {
480  // invalid URL, save to default location
481  ROS_ERROR_STREAM("invalid url: " << resURL << " (ignored)");
482  success = saveCalibration(new_info, default_camera_info_url, cname);
483  break;
484  }
485  }
486 
487  return success;
488 }
489 
499 bool
500 CameraInfoManager::saveCalibrationFile(const sensor_msgs::CameraInfo &new_info,
501  const std::string &filename,
502  const std::string &cname)
503 {
504  ROS_INFO_STREAM("writing calibration data to " << filename);
505 
506  // isolate the name of the containing directory
507  size_t last_slash = filename.rfind('/');
508  if (last_slash >= filename.length())
509  {
510  // No slash in the name. This should never happen, the URL
511  // parser ensures there is at least one '/' at the beginning.
512  ROS_ERROR_STREAM("filename [" << filename << "] has no '/'");
513  return false; // not a valid URL
514  }
515 
516  // make sure the directory exists and is writable
517  std::string dirname(filename.substr(0, last_slash+1));
518  struct stat stat_data;
519  int rc = stat(dirname.c_str(), &stat_data);
520  if (rc != 0)
521  {
522  if (errno == ENOENT)
523  {
524  // directory does not exist, try to create it and its parents
525  std::string command("mkdir -p " + dirname);
526  rc = system(command.c_str());
527  if (rc != 0)
528  {
529  // mkdir failed
530  ROS_ERROR_STREAM("unable to create path to directory ["
531  << dirname << "]");
532  return false;
533  }
534  }
535  else
536  {
537  // not accessible, or something screwy
538  ROS_ERROR_STREAM("directory [" << dirname << "] not accessible");
539  return false;
540  }
541  }
542  else if (!S_ISDIR(stat_data.st_mode))
543  {
544  // dirname exists but is not a directory
545  ROS_ERROR_STREAM("[" << dirname << "] is not a directory");
546  return false;
547  }
548 
549  // Directory exists and is accessible. Permissions might still be bad.
550 
551  // Currently, writeCalibration() always returns true no matter what
552  // (ros-pkg ticket #5010).
553  return writeCalibration(filename, cname, new_info);
554 }
555 
564 bool
565 CameraInfoManager::setCameraInfoService(sensor_msgs::SetCameraInfo::Request &req,
566  sensor_msgs::SetCameraInfo::Response &rsp)
567 {
568  // copies of class variables needed for saving calibration
569  std::string url_copy;
570  std::string cname;
571  {
572  boost::mutex::scoped_lock lock(mutex_);
573  cam_info_ = req.camera_info;
574  url_copy = url_;
575  cname = camera_name_;
576  loaded_cam_info_ = true;
577  }
578 
579  if (!nh_.ok())
580  {
581  ROS_ERROR("set_camera_info service called, but driver not running.");
582  rsp.status_message = "Camera driver not running.";
583  rsp.success = false;
584  return false;
585  }
586 
587  rsp.success = saveCalibration(req.camera_info, url_copy, cname);
588  if (!rsp.success)
589  rsp.status_message = "Error storing camera calibration.";
590 
591  return true;
592 }
593 
604 bool CameraInfoManager::setCameraName(const std::string &cname)
605 {
606  // the camera name may not be empty
607  if (cname.empty())
608  return false;
609 
610  // validate the camera name characters
611  for (unsigned i = 0; i < cname.size(); ++i)
612  {
613  if (!isalnum(cname[i]) && cname[i] != '_')
614  return false;
615  }
616 
617  // The name is valid, so update our private copy. Since the new
618  // name might cause the existing URL to resolve somewhere else,
619  // force @c cam_info_ to be reloaded before being used again.
620  {
621  boost::mutex::scoped_lock lock(mutex_);
622  camera_name_ = cname;
623  loaded_cam_info_ = false;
624  }
625 
626  return true;
627 }
628 
637 bool CameraInfoManager::setCameraInfo(const sensor_msgs::CameraInfo &camera_info)
638 {
639  boost::mutex::scoped_lock lock(mutex_);
640 
641  cam_info_ = camera_info;
642  loaded_cam_info_ = true;
643 
644  return true;
645 }
646 
654 bool CameraInfoManager::validateURL(const std::string &url)
655 {
656  std::string cname; // copy of camera name
657  {
658  boost::mutex::scoped_lock lock(mutex_);
659  cname = camera_name_;
660  } // release the lock
661 
662  url_type_t url_type = parseURL(resolveURL(url, cname));
663  return (url_type < URL_invalid);
664 }
665 
666 } // namespace camera_info_manager
bool saveCalibration(const sensor_msgs::CameraInfo &new_info, const std::string &url, const std::string &cname)
sensor_msgs::CameraInfo getCameraInfo(void)
string package
bool loaded_cam_info_
cam_info_ load attempted
def readCalibration(file_name)
std::string url_
URL for calibration data.
std::string getPackageFileName(const std::string &url)
bool loadCameraInfo(const std::string &url)
std::string resolveURL(const std::string &url, const std::string &cname)
ros::ServiceServer info_service_
set_camera_info service
sensor_msgs::CameraInfo cam_info_
current CameraInfo
CameraInfo Manager interface.
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
const std::string default_camera_info_url
bool loadCalibrationFile(const std::string &filename, const std::string &cname)
#define ROS_WARN(...)
url_type_t parseURL(const std::string &url)
bool validateURL(const std::string &url)
boost::mutex mutex_
mutual exclusion lock for private data
CameraInfoManager(ros::NodeHandle nh, const std::string &cname="camera", const std::string &url="")
ROSLIB_DECL std::string command(const std::string &cmd)
#define ROS_INFO(...)
ROSCPP_DECL bool ok()
bool writeCalibration(const std::string &file_name, const std::string &camera_name, const sensor_msgs::CameraInfo &cam_info)
ros::NodeHandle nh_
node handle for service
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
ROSLIB_DECL std::string getPath(const std::string &package_name)
#define ROS_INFO_STREAM(args)
bool setCameraInfo(const sensor_msgs::CameraInfo &camera_info)
bool loadCalibration(const std::string &url, const std::string &cname)
#define ROS_ERROR_STREAM(args)
bool ok() const
#define ROS_ERROR(...)
bool setCameraInfoService(sensor_msgs::SetCameraInfo::Request &req, sensor_msgs::SetCameraInfo::Response &rsp)
bool saveCalibrationFile(const sensor_msgs::CameraInfo &new_info, const std::string &filename, const std::string &cname)
bool setCameraName(const std::string &cname)


camera_info_manager
Author(s): Jack O'Quin
autogenerated on Mon Feb 28 2022 22:31:54