CameraInfo Manager class. More...
#include <camera_info_manager.h>
Public Member Functions | |
CameraInfoManager (ros::NodeHandle nh, const std::string &cname="camera", const std::string &url="") | |
sensor_msgs::CameraInfo | getCameraInfo (void) |
bool | isCalibrated (void) |
bool | loadCameraInfo (const std::string &url) |
std::string | resolveURL (const std::string &url, const std::string &cname) |
bool | setCameraName (const std::string &cname) |
bool | validateURL (const std::string &url) |
Private Types | |
enum | url_type_t { URL_empty = 0, URL_file, URL_package, URL_invalid, URL_flash } |
Private Member Functions | |
std::string | getPackageFileName (const std::string &url) |
bool | loadCalibration (const std::string &url, const std::string &cname) |
bool | loadCalibrationFile (const std::string &filename, const std::string &cname) |
url_type_t | parseURL (const std::string &url) |
bool | saveCalibration (const sensor_msgs::CameraInfo &new_info, const std::string &url, const std::string &cname) |
bool | saveCalibrationFile (const sensor_msgs::CameraInfo &new_info, const std::string &filename, const std::string &cname) |
bool | setCameraInfo (sensor_msgs::SetCameraInfo::Request &req, sensor_msgs::SetCameraInfo::Response &rsp) |
Private Attributes | |
sensor_msgs::CameraInfo | cam_info_ |
current CameraInfo | |
std::string | camera_name_ |
camera name | |
ros::ServiceServer | info_service_ |
set_camera_info service | |
bool | loaded_cam_info_ |
cam_info_ load attempted | |
boost::mutex | mutex_ |
mutual exclusion lock for private data | |
ros::NodeHandle | nh_ |
node handle for service | |
std::string | url_ |
URL for calibration data. |
CameraInfo Manager class.
Provides CameraInfo, handles the sensor_msgs/SetCameraInfo service requests, saves and restores sensor_msgs/CameraInfo data.
Typically, these service requests are made by a calibration package, such as:
The calling node must invoke ros::spin() or ros::spinOnce() in some thread, so CameraInfoManager can handle arriving service requests.
The device driver sets a camera name via the CameraInfoManager::CameraInfoManager constructor or the setCameraName() method. This name is written when CameraInfo is saved, and checked when data are loaded, with a warning logged if the name read does not match.
Syntax: a camera name contains any combination of alphabetic, numeric and '_' characters. Case is significant.
Camera drivers may use any syntactically valid name they please. Where possible, it is best for the name to be unique to the device, such as a GUID, or the make, model and serial number. Any parameters that affect calibration, such as resolution, focus, zoom, etc., may also be included in the name, uniquely identifying each CameraInfo file.
Beginning with Electric Emys, the camera name can be resolved as part of the URL, allowing direct access to device-specific calibration information.
The location for getting and saving calibration data is expressed by Uniform Resource Locator. The driver defines a URL via the CameraInfoManager::CameraInfoManager constructor or the loadCameraInfo() method. Many drivers provide a ~camera_info_url
parameter so users may customize this URL, but that is handled outside this class.
Typically, cameras store calibration information in a file, which can be in any format supported by camera_calibration_parsers
. Currently, that includes YAML and Videre INI files, identified by their .yaml or .ini extensions as shown in the examples. These file formats are described here:
Example URL syntax:
The file:
URL specifies a full path name in the local system. The package:
URL is handled the same as file:,
except the path name is resolved relative to the location of the named ROS package, which must be reachable via $ROS_PACKAGE_PATH
.
Beginning with Electric Emys, the URL may contain substitution variables delimited by ${...}
, including:
${NAME}
resolved to the current camera name defined by the device driver.${ROS_HOME}
resolved to the $ROS_HOME
environment variable if defined, ~/.ros
if not.Resolution is done in a single pass through the URL string. Variable values containing substitutable strings are not resolved recursively. Unrecognized variable names are treated literally with no substitution, but an error is logged.
Examples with variable substitution:
In C-turtle and Diamondback, if the URL was empty, no calibration data were loaded, and any data provided via `set_camera_info` would be stored in:
Beginning in Electric, the default URL changed to:
If that file exists, its contents are used. Any new calibration will be stored there, missing parent directories being created if necessary and possible.
Prior to Fuerte, calibration information was loaded in the constructor, and again each time the URL or camera name was updated. This frequently caused logging of confusing and misleading error messages.
Beginning in Fuerte, camera_info_manager loads nothing until the loadCameraInfo()
, isCalibrated()
or getCameraInfo()
method is called. That suppresses bogus error messages, but allows (valid) load errors to occur during the first getCameraInfo()
, or isCalibrated()
. To avoid that, do an explicit loadCameraInfo()
first.
Definition at line 174 of file camera_info_manager.h.
enum camera_info_manager::CameraInfoManager::url_type_t [private] |
Definition at line 193 of file camera_info_manager.h.
camera_info_manager::CameraInfoManager::CameraInfoManager | ( | ros::NodeHandle | nh, |
const std::string & | cname = "camera" , |
||
const std::string & | url = "" |
||
) |
Constructor
nh | node handle, normally for the driver's streaming name space ("camera"). The service name is relative to this handle. Nodes supporting multiple cameras may use subordinate names, like "left/camera" and "right/camera". |
cname | default camera name |
url | default Uniform Resource Locator for loading and saving data. |
Definition at line 78 of file camera_info_manager.cpp.
sensor_msgs::CameraInfo camera_info_manager::CameraInfoManager::getCameraInfo | ( | void | ) |
Get the current CameraInfo data.
If CameraInfo has not yet been loaded, an attempt must be made here. To avoid that, ensure that loadCameraInfo() ran previously. If the load is attempted but fails, an empty CameraInfo will be supplied.
The matrices are all zeros if no calibration is available. The image pipeline handles that as uncalibrated data.
Definition at line 106 of file camera_info_manager.cpp.
std::string camera_info_manager::CameraInfoManager::getPackageFileName | ( | const std::string & | url | ) | [private] |
Get file name corresponding to a package:
URL.
url | a copy of the Uniform Resource Locator |
Definition at line 138 of file camera_info_manager.cpp.
bool camera_info_manager::CameraInfoManager::isCalibrated | ( | void | ) |
Is the current CameraInfo calibrated?
If CameraInfo has not yet been loaded, an attempt must be made here. To avoid that, ensure that loadCameraInfo() ran previously. If the load failed, CameraInfo will be empty and this predicate will return false.
Definition at line 171 of file camera_info_manager.cpp.
bool camera_info_manager::CameraInfoManager::loadCalibration | ( | const std::string & | url, |
const std::string & | cname | ||
) | [private] |
Load CameraInfo calibration data (if any).
url | a copy of the Uniform Resource Locator |
cname | is a copy of the camera_name_ |
sets cam_info_, if successful
Definition at line 208 of file camera_info_manager.cpp.
bool camera_info_manager::CameraInfoManager::loadCalibrationFile | ( | const std::string & | filename, |
const std::string & | cname | ||
) | [private] |
Load CameraInfo calibration data from a file.
filename | containing CameraInfo to read |
cname | is a copy of the camera_name_ |
Sets cam_info_, if successful
Definition at line 266 of file camera_info_manager.cpp.
bool camera_info_manager::CameraInfoManager::loadCameraInfo | ( | const std::string & | url | ) |
Set a new URL and load its calibration data (if any).
If multiple threads call this method simultaneously with different URLs, there is no guarantee which will prevail.
url | new Uniform Resource Locator for CameraInfo. |
loaded_cam_info_
true (meaning a load was attempted, even if it failed); cam_info_
updated, if successful. Definition at line 308 of file camera_info_manager.cpp.
CameraInfoManager::url_type_t camera_info_manager::CameraInfoManager::parseURL | ( | const std::string & | url | ) | [private] |
Parse calibration Uniform Resource Locator.
url | string to parse |
Definition at line 405 of file camera_info_manager.cpp.
std::string camera_info_manager::CameraInfoManager::resolveURL | ( | const std::string & | url, |
const std::string & | cname | ||
) |
Resolve Uniform Resource Locator string.
url | a copy of the Uniform Resource Locator, which may include ${...} substitution variables. |
cname | is a copy of the camera_name_ |
Definition at line 331 of file camera_info_manager.cpp.
bool camera_info_manager::CameraInfoManager::saveCalibration | ( | const sensor_msgs::CameraInfo & | new_info, |
const std::string & | url, | ||
const std::string & | cname | ||
) | [private] |
Save CameraInfo calibration data.
new_info | contains CameraInfo to save |
url | is a copy of the URL storage location (if empty, use file://${ROS_HOME}/camera_info/${NAME}.yaml ) |
cname | is a copy of the camera_name_ |
Definition at line 441 of file camera_info_manager.cpp.
bool camera_info_manager::CameraInfoManager::saveCalibrationFile | ( | const sensor_msgs::CameraInfo & | new_info, |
const std::string & | filename, | ||
const std::string & | cname | ||
) | [private] |
Save CameraInfo calibration data to a file.
new_info | contains CameraInfo to save |
filename | is local file to store data |
cname | is a copy of the camera_name_ |
Definition at line 491 of file camera_info_manager.cpp.
bool camera_info_manager::CameraInfoManager::setCameraInfo | ( | sensor_msgs::SetCameraInfo::Request & | req, |
sensor_msgs::SetCameraInfo::Response & | rsp | ||
) | [private] |
Callback for SetCameraInfo request.
Always updates cam_info_ class variable, even if save fails.
req | SetCameraInfo request message |
rsp | SetCameraInfo response message |
Definition at line 556 of file camera_info_manager.cpp.
bool camera_info_manager::CameraInfoManager::setCameraName | ( | const std::string & | cname | ) |
Set a new camera name.
cname | new camera name to use for saving calibration data |
cam_name_
updated, if valid; since it may affect the URL, cam_info_
will be reloaded before being used again. Definition at line 595 of file camera_info_manager.cpp.
bool camera_info_manager::CameraInfoManager::validateURL | ( | const std::string & | url | ) |
Validate URL syntax.
url | Uniform Resource Locator to check |
Definition at line 627 of file camera_info_manager.cpp.
sensor_msgs::CameraInfo camera_info_manager::CameraInfoManager::cam_info_ [private] |
current CameraInfo
Definition at line 235 of file camera_info_manager.h.
std::string camera_info_manager::CameraInfoManager::camera_name_ [private] |
camera name
Definition at line 233 of file camera_info_manager.h.
set_camera_info service
Definition at line 232 of file camera_info_manager.h.
bool camera_info_manager::CameraInfoManager::loaded_cam_info_ [private] |
cam_info_ load attempted
Definition at line 236 of file camera_info_manager.h.
boost::mutex camera_info_manager::CameraInfoManager::mutex_ [private] |
mutual exclusion lock for private data
This non-recursive mutex is only held for a short time while accessing or changing private class variables. To avoid deadlocks and contention, it is never held during I/O or while invoking a callback. Most private methods operate on copies of class variables, keeping the mutex hold time short.
Definition at line 228 of file camera_info_manager.h.
node handle for service
Definition at line 231 of file camera_info_manager.h.
std::string camera_info_manager::CameraInfoManager::url_ [private] |
URL for calibration data.
Definition at line 234 of file camera_info_manager.h.