From 75a89a65ecb644bf8a18d33b36e1aebf5ad96e57 Mon Sep 17 00:00:00 2001 From: Alejandro Hernandez Cordero Date: Fri, 17 Apr 2026 14:20:56 +0200 Subject: [PATCH] Added rosdoc2 documentation to camera info manager Signed-off-by: Alejandro Hernandez Cordero --- camera_info_manager/doc/api.rst | 93 +++++++ camera_info_manager/doc/conf.py | 35 +++ camera_info_manager/doc/overview.rst | 120 ++++++++ .../camera_info_manager.hpp | 256 +++++++++--------- camera_info_manager/package.xml | 10 +- camera_info_manager/rosdoc2.yaml | 30 ++ 6 files changed, 412 insertions(+), 132 deletions(-) create mode 100644 camera_info_manager/doc/api.rst create mode 100644 camera_info_manager/doc/conf.py create mode 100644 camera_info_manager/doc/overview.rst create mode 100644 camera_info_manager/rosdoc2.yaml diff --git a/camera_info_manager/doc/api.rst b/camera_info_manager/doc/api.rst new file mode 100644 index 00000000..e0f101a0 --- /dev/null +++ b/camera_info_manager/doc/api.rst @@ -0,0 +1,93 @@ +API Reference +============= + +CameraInfoManager +----------------- + +:cpp:class:`camera_info_manager::CameraInfoManager` is the main class. +Camera drivers create one instance per camera and use it to serve calibration +data. + +See the full :ref:`exhale_class_classcamera__info__manager_1_1CameraInfoManager` +API reference. + +Type Aliases +------------ + +The following type aliases are defined in the ``camera_info_manager`` namespace: + +- ``CameraInfo`` — alias for ``sensor_msgs::msg::CameraInfo``. +- ``SetCameraInfo`` — alias for ``sensor_msgs::srv::SetCameraInfo``. + +Usage Example +------------- + +**Minimal camera driver** + +.. code-block:: cpp + + #include + #include + #include + + class MyCameraDriver : public rclcpp::Node + { + public: + MyCameraDriver() + : Node("my_camera") + { + cim_ = std::make_shared( + get_node_base_interface(), + get_node_services_interface(), + get_node_logging_interface(), + "my_camera", // camera name + "package://my_driver/config/cam.yaml" // calibration URL + ); + + // Eagerly load calibration so errors surface before streaming starts. + if (!cim_->loadCameraInfo("")) { + RCLCPP_WARN(get_logger(), "No calibration loaded"); + } + + auto it = image_transport::ImageTransport(shared_from_this()); + cam_pub_ = it.advertiseCamera("image_raw", 1); + + timer_ = create_wall_timer( + std::chrono::milliseconds(33), + std::bind(&MyCameraDriver::publishFrame, this)); + } + + private: + void publishFrame() + { + auto image = std::make_shared(); + // ... fill image from hardware ... + + auto ci = std::make_shared( + cim_->getCameraInfo()); + ci->header = image->header; + cam_pub_.publish(image, ci); + } + + std::shared_ptr cim_; + image_transport::CameraPublisher cam_pub_; + rclcpp::TimerBase::SharedPtr timer_; + }; + +**Changing the calibration URL at runtime** + +.. code-block:: cpp + + // Reload from a different file without restarting the node: + cim_->loadCameraInfo("file:///tmp/new_calibration.yaml"); + +**Checking calibration status** + +.. code-block:: cpp + + if (cim_->isCalibrated()) { + auto ci = cim_->getCameraInfo(); + RCLCPP_INFO(get_logger(), "fx = %f", ci.k[0]); + } else { + RCLCPP_WARN(get_logger(), "Camera not calibrated"); + } diff --git a/camera_info_manager/doc/conf.py b/camera_info_manager/doc/conf.py new file mode 100644 index 00000000..a5b42f98 --- /dev/null +++ b/camera_info_manager/doc/conf.py @@ -0,0 +1,35 @@ +import os +import sys + +sys.path.insert(0, os.path.abspath('.')) + +project = 'camera_info_manager' +copyright = '2024, Open Robotics' +author = 'Alejandro Hernandez Cordero, Geoffrey Biggs, Jack O\'Quin, Michael Carroll' + +extensions = [ + 'sphinx.ext.autodoc', + 'sphinx.ext.intersphinx', + 'breathe', + 'myst_parser', +] + +# breathe_projects and breathe_default_project are overridden by rosdoc2 at build time. +breathe_default_project = 'camera_info_manager Doxygen Project' +breathe_default_members = ('members', 'undoc-members') + +source_suffix = { + '.rst': 'restructuredtext', + '.md': 'markdown', +} + +templates_path = ['_templates'] +# Exclude root-level copies of user docs (rosdoc2 also copies them to user_docs/ +# where they are covered by the user_docs.rst glob toctree). +exclude_patterns = [ + '_build', + 'overview.rst', + 'api.rst', +] + +html_theme = 'sphinx_rtd_theme' diff --git a/camera_info_manager/doc/overview.rst b/camera_info_manager/doc/overview.rst new file mode 100644 index 00000000..be5efc82 --- /dev/null +++ b/camera_info_manager/doc/overview.rst @@ -0,0 +1,120 @@ +Overview +======== + +``camera_info_manager`` provides a C++ class that manages +``sensor_msgs/msg/CameraInfo`` for camera drivers. It handles: + +- **Loading** calibration data from a file or ``package://`` URL on first + access (lazy loading). +- **Saving** calibration data to a file in response to a + ``sensor_msgs/srv/SetCameraInfo`` service call. +- **Advertising** the ``set_camera_info`` service so calibration tools can + push new calibration data to a running camera driver. + +Architecture +------------ + +A camera driver creates a ``CameraInfoManager`` at startup, passing node +interfaces and a calibration URL. The manager advertises the +``set_camera_info`` service and lazy-loads calibration data the first time +``getCameraInfo()`` or ``isCalibrated()`` is called. + +.. code-block:: none + + Camera Driver Node + ├── CameraInfoManager + │ ├── Reads calibration URL → loads CameraInfo from file/package + │ ├── Serves ~/set_camera_info → saves new calibration to file + │ └── Returns CameraInfo via getCameraInfo() + └── Publishes Image + CameraInfo on every frame + +Calibration URL Schemes +----------------------- + +``camera_info_manager`` supports two URL schemes for locating calibration +files: + +``file://`` + Absolute path on the local filesystem. + + .. code-block:: none + + file:///home/user/calibrations/camera.yaml + +``package://`` + Path relative to the install prefix of a ROS 2 package. + + .. code-block:: none + + package://my_camera_driver/config/calibration.yaml + +URL Variable Substitution +~~~~~~~~~~~~~~~~~~~~~~~~~ + +URLs may contain the following variables (resolved in a single pass): + +- ``${NAME}`` — replaced with the camera name set at construction time or + via ``setCameraName()``. +- ``${ROS_HOME}`` — replaced with the value of the ``$ROS_HOME`` environment + variable, or ``~/.ros`` if unset. + +Default URL (when none is provided): + +.. code-block:: none + + file://${ROS_HOME}/camera_info/${NAME}.yaml + +Camera Name +----------- + +The camera name is used for two purposes: + +1. Substituted for ``${NAME}`` in the calibration URL. +2. Compared against the camera name stored inside the calibration file — a + warning is logged if they differ. + +A valid camera name consists of alphanumeric characters and underscores. +For uniqueness, consider encoding a GUID or make/model/serial combination. + +Lazy Loading +------------ + +No I/O is performed in the constructor. Calibration data are read on the +first call to ``getCameraInfo()``, ``isCalibrated()``, or an explicit +``loadCameraInfo()``. Call ``loadCameraInfo()`` early in the driver's +``on_activate()`` or equivalent lifecycle callback to surface load errors +before streaming begins. + +ROS 2 Service +------------- + +The manager advertises ``~/set_camera_info`` (relative to the node namespace +unless overridden). Calibration tools such as +``camera_calibration`` send new calibration data via this service; the manager +saves the data to the configured URL and updates the in-memory CameraInfo. + +Lifecycle Node Support +---------------------- + +Pass a ``rclcpp_lifecycle::LifecycleNode *`` to the deprecated constructor, or +extract node interfaces from the lifecycle node and use the preferred +interface-based constructor. + +.. code-block:: cpp + + auto cim = std::make_shared( + node->get_node_base_interface(), + node->get_node_services_interface(), + node->get_node_logging_interface(), + "my_camera", + "package://my_driver/config/camera.yaml"); + +Dependencies +------------ + +- ``rclcpp`` — node interfaces and service server. +- ``rclcpp_lifecycle`` — lifecycle node support (deprecated constructor path). +- ``sensor_msgs`` — ``CameraInfo`` and ``SetCameraInfo`` types. +- ``camera_calibration_parsers`` — YAML / INI calibration file I/O. +- ``ament_index_cpp`` — resolves ``package://`` URLs to filesystem paths. +- ``rcpputils`` — utility helpers. diff --git a/camera_info_manager/include/camera_info_manager/camera_info_manager.hpp b/camera_info_manager/include/camera_info_manager/camera_info_manager.hpp index 05ad73b4..d694508f 100644 --- a/camera_info_manager/include/camera_info_manager/camera_info_manager.hpp +++ b/camera_info_manager/include/camera_info_manager/camera_info_manager.hpp @@ -54,134 +54,69 @@ namespace camera_info_manager using CameraInfo = sensor_msgs::msg::CameraInfo; using SetCameraInfo = sensor_msgs::srv::SetCameraInfo; -/** @brief CameraInfo Manager class - - Provides CameraInfo, handles the sensor_msgs/SetCameraInfo service - requests, saves and restores sensor_msgs/CameraInfo data. - - @par ROS Service - - - @b set_camera_info (sensor_msgs/SetCameraInfo) stores - calibration information - - Typically, these service requests are made by a calibration - package, such as: - - - http://www.ros.org/wiki/camera_calibration - - The calling node @em must invoke ros::spin() or ros::spinOnce() in - some thread, so CameraInfoManager can handle arriving service - requests. - - @par Camera Name - - 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. - - @par Uniform Resource Locator - - 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 @c - ~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 @c 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: - - - http://www.ros.org/wiki/camera_calibration_parsers#File_formats - - Example URL syntax: - - - file:///full/path/to/local/file.yaml - - file:///full/path/to/videre/file.ini - - package://camera_info_manager/tests/test_calibration.yaml - - package://ros_package_name/calibrations/camera3.yaml - - The @c file: URL specifies a full path name in the local system. - The @c package: URL is handled the same as @c file:, except the - path name is resolved relative to the location of the named ROS - package, which @em must be reachable via @c $ROS_PACKAGE_PATH. - - Beginning with Electric Emys, the URL may contain substitution - variables delimited by ${...}, including: - - - @c ${NAME} resolved to the current camera name defined by the - device driver. - - @c ${ROS_HOME} resolved to the @c $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: - - - package://my_cameras/calibrations/${NAME}.yaml - - file://${ROS_HOME}/camera_info/left_front_camera.yaml - - 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: - - - file:///tmp/calibration_${NAME}.yaml - - Beginning in Electric, the default URL changed to: - - - file://${ROS_HOME}/camera_info/${NAME}.yaml. - - If that file exists, its contents are used. Any new calibration - will be stored there, missing parent directories being created if - necessary and possible. - - @par Namespace - - The CameraInfoManager constructor takes an optional namespace - argument, which is used to set the ROS namespace for the - set_camera_info service. If not provided, the namespace defaults - to the private namespace of the node (i.e., "~/set_camera_info"). - - @par Loading Calibration Data - - 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 - @c loadCameraInfo(), @c isCalibrated() or @c getCameraInfo() - method is called. That suppresses bogus error messages, but allows - (valid) load errors to occur during the first @c getCameraInfo(), - or @c isCalibrated(). To avoid that, do an explicit @c - loadCameraInfo() first. - -*/ +/** + * \brief Manages camera calibration data for a ROS 2 camera driver. + * + * Provides \c sensor_msgs::msg::CameraInfo, handles the + * \c sensor_msgs::srv::SetCameraInfo service, and saves/restores calibration + * data from the filesystem. + * + * \par ROS 2 Service + * + * - \b set_camera_info (sensor_msgs/SetCameraInfo) — stores calibration + * information sent by a calibration tool. + * + * \par Camera Name + * + * A camera name is set via the constructor or setCameraName(). It is written + * into saved calibration files and checked on load; a warning is logged when + * the name does not match. + * + * Syntax: any combination of alphanumeric characters and underscores. + * Case is significant. For uniqueness, consider using a GUID or a string + * that encodes make, model and serial number. + * + * \par Calibration URL + * + * The location for reading and writing calibration data is expressed as a URL. + * Supported schemes: + * + * - \c file:///full/path/to/calibration.yaml + * - \c package://ros_package_name/calibrations/camera.yaml + * + * The \c package: scheme resolves the path relative to the install prefix of + * the named ROS package. + * + * URL variables (resolved in a single pass, not recursively): + * + * - \c ${NAME} — the current camera name. + * - \c ${ROS_HOME} — value of the \c $ROS_HOME environment variable, + * or \c ~/.ros if unset. + * + * The default URL when none is given is: + * \code{.none} + * file://${ROS_HOME}/camera_info/${NAME}.yaml + * \endcode + * If that file exists it is loaded automatically. + * + * \par Lazy Loading + * + * No calibration data are loaded in the constructor. The first call to + * getCameraInfo(), isCalibrated(), or an explicit loadCameraInfo() triggers + * the actual file read. Call loadCameraInfo() eagerly to surface load errors + * before the first getCameraInfo() call. + * + * \par Namespace + * + * The optional \p ns constructor argument overrides the ROS 2 namespace used + * for the \c set_camera_info service. The default is the node's private + * namespace (\c ~/set_camera_info). + */ class CameraInfoManager { public: + /// \brief Construct from a raw rclcpp::Node pointer (deprecated). [[deprecated("Use CameraInfoManager(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr, ...)" " instead.")]] CAMERA_INFO_MANAGER_PUBLIC @@ -191,6 +126,7 @@ class CameraInfoManager const std::string & url = "", const std::string & ns = ""); + /// \brief Construct from a raw rclcpp_lifecycle::LifecycleNode pointer (deprecated). [[deprecated("Use CameraInfoManager(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr, ...)" " instead.")]] CAMERA_INFO_MANAGER_PUBLIC @@ -200,6 +136,7 @@ class CameraInfoManager const std::string & url = "", const std::string & ns = ""); + /// \brief Construct from node interfaces with legacy rmw_qos_profile_t QoS (deprecated). [[deprecated("Use CameraInfoManager(..., rclcpp::QoS, ...) instead")]] CAMERA_INFO_MANAGER_PUBLIC CameraInfoManager( @@ -210,6 +147,20 @@ class CameraInfoManager rmw_qos_profile_t custom_qos = rmw_qos_profile_default, const std::string & ns = ""); + /** + * \brief Preferred constructor — takes node interfaces and an rclcpp::QoS. + * + * \param node_base_interface Base interface of the owning node. + * \param node_services_interface Services interface of the owning node. + * \param node_logger_interface Logging interface of the owning node. + * \param cname Camera name used for URL variable substitution and + * calibration file validation. + * \param url URL of the calibration file to load on first access. + * Defaults to \c file://${ROS_HOME}/camera_info/${NAME}.yaml. + * \param custom_qos QoS profile for the \c set_camera_info service. + * \param ns ROS 2 namespace for the \c set_camera_info service. + * Defaults to the node's private namespace. + */ CAMERA_INFO_MANAGER_PUBLIC CameraInfoManager( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, @@ -219,26 +170,83 @@ class CameraInfoManager rclcpp::QoS custom_qos = rclcpp::SystemDefaultsQoS(), const std::string & ns = ""); + /** + * \brief Return the current CameraInfo, loading calibration data on first call. + * + * \return A copy of the current \c sensor_msgs::msg::CameraInfo. + * If calibration has not been loaded yet this call triggers the load. + * Returns an empty (uncalibrated) CameraInfo on load failure. + */ CAMERA_INFO_MANAGER_PUBLIC CameraInfo getCameraInfo(void); + /** + * \brief Check whether valid calibration data have been loaded. + * + * Triggers a lazy load if calibration has not been attempted yet. + * + * \return \c true if calibration data are available. + */ CAMERA_INFO_MANAGER_PUBLIC bool isCalibrated(void); + /** + * \brief Load calibration data from a URL, replacing any previously loaded data. + * + * \param url URL of the calibration file to load (see class documentation + * for supported URL schemes and variable substitution). + * \return \c true on success. + */ CAMERA_INFO_MANAGER_PUBLIC bool loadCameraInfo(const std::string & url); + /** + * \brief Resolve URL substitution variables for a given camera name. + * + * Substitutes \c ${NAME} and \c ${ROS_HOME} in \p url using \p cname and + * the environment. + * + * \param url URL string that may contain substitution variables. + * \param cname Camera name to substitute for \c ${NAME}. + * \return The resolved URL string. + */ CAMERA_INFO_MANAGER_PUBLIC std::string resolveURL( const std::string & url, const std::string & cname); + /** + * \brief Set the camera name used for URL variable substitution. + * + * Updates the stored camera name. If calibration data have already been + * loaded, this does not automatically reload them. + * + * \param cname New camera name (alphanumeric and underscores only). + * \return \c true if \p cname is a syntactically valid camera name. + */ CAMERA_INFO_MANAGER_PUBLIC bool setCameraName(const std::string & cname); + /** + * \brief Directly set (and save) camera calibration data. + * + * Stores \p camera_info as the current calibration and attempts to write it + * to the configured URL. + * + * \param camera_info Calibration data to store. + * \return \c true if the data were saved successfully. + */ CAMERA_INFO_MANAGER_PUBLIC bool setCameraInfo(const CameraInfo & camera_info); + /** + * \brief Check whether a URL string has a supported scheme. + * + * Does not attempt to open or read the resource. + * + * \param url URL to validate. + * \return \c true if the URL scheme is recognised (\c file: or \c package:). + */ CAMERA_INFO_MANAGER_PUBLIC bool validateURL(const std::string & url); diff --git a/camera_info_manager/package.xml b/camera_info_manager/package.xml index da787b09..6dcb589b 100644 --- a/camera_info_manager/package.xml +++ b/camera_info_manager/package.xml @@ -1,14 +1,7 @@ camera_info_manager 6.4.7 - - - This package provides a C++ interface for camera calibration - information. It provides CameraInfo, and handles SetCameraInfo - service requests, saving and restoring the camera calibration - data. - - + C++ interface for camera calibration information: loads, stores and serves CameraInfo via the SetCameraInfo service. Jack O'Quin Michael Carroll @@ -37,6 +30,7 @@ ament_cmake + rosdoc2.yaml diff --git a/camera_info_manager/rosdoc2.yaml b/camera_info_manager/rosdoc2.yaml new file mode 100644 index 00000000..a16ca371 --- /dev/null +++ b/camera_info_manager/rosdoc2.yaml @@ -0,0 +1,30 @@ +## Default configuration, generated by rosdoc2. + +## This 'attic section' self-documents this file's type and version. +type: 'rosdoc2 config' +version: 1 + +--- + +settings: + always_run_doxygen: true + +builders: + - doxygen: { + name: 'camera_info_manager Public C/C++ API', + output_dir: 'generated/doxygen', + extra_doxyfile_statements: [ + 'PREDEFINED = DOXYGEN_SHOULD_SKIP_THIS CAMERA_INFO_MANAGER_PUBLIC= CAMERA_INFO_MANAGER_LOCAL= CAMERA_INFO_MANAGER_PUBLIC_TYPE= CAMERA_INFO_MANAGER_EXPORT= CAMERA_INFO_MANAGER_IMPORT=', + 'EXTRACT_ALL = YES', + 'EXTRACT_PRIVATE = NO', + 'EXTRACT_STATIC = YES', + 'ENABLE_PREPROCESSING = YES', + 'QUIET = YES' + ] + } + - sphinx: { + name: 'camera_info_manager', + sphinx_sourcedir: doc, + doxygen_xml_directory: 'generated/doxygen/xml', + output_dir: '' + }