diff --git a/camera_info_manager_py/camera_info_manager/camera_info_manager.py b/camera_info_manager_py/camera_info_manager/camera_info_manager.py index 1360e289..ffcebc7a 100644 --- a/camera_info_manager_py/camera_info_manager/camera_info_manager.py +++ b/camera_info_manager_py/camera_info_manager/camera_info_manager.py @@ -83,8 +83,8 @@ class CameraInfoManager: CameraInfoManager class. :class:`CameraInfoManager` provides ROS CameraInfo support for - Python camera drivers. It handles the `sensor_msgs/SetCameraInfo`_ - service requests, saving and restoring `sensor_msgs/CameraInfo`_ + Python camera drivers. It handles the ``sensor_msgs/SetCameraInfo`` + service requests, saving and restoring ``sensor_msgs/CameraInfo`` data. :param cname: camera name. @@ -100,7 +100,7 @@ class CameraInfoManager: **ROS Service** - - set_camera_info (`sensor_msgs/SetCameraInfo`_) stores + - set_camera_info (``sensor_msgs/SetCameraInfo``) stores calibration information Typically, these service requests are made by a calibration @@ -191,7 +191,7 @@ class CameraInfoManager: **Loading Calibration Data** - Unlike the `C++ camera_info_manager`_, this Python implementation + Unlike the C++ camera_info_manager, this Python implementation loads nothing until the :py:meth:`loadCameraInfo` method is called. It is an error to call :py:meth:`getCameraInfo`, or :py:meth:`isCalibrated` before that is done. @@ -202,7 +202,16 @@ class CameraInfoManager: """ def __init__(self, node: Node, cname='camera', url='', namespace=''): - """Call the Constructor.""" + """ + Initialise the manager and advertise the set_camera_info service. + + :param node: ROS 2 node used to create the service server. + :param cname: Camera name (alphanumeric and underscores only). + :param url: Calibration URL. Defaults to + ``file://${ROS_HOME}/camera_info/${NAME}.yaml``. + :param namespace: Optional ROS namespace prefix for the + ``set_camera_info`` service name. + """ self.node = node self.cname = cname self.url = url @@ -230,7 +239,7 @@ def getCameraInfo(self): The :py:meth:`loadCameraInfo` must have been called since the last time the camera name or URL changed. - :returns: `sensor_msgs/CameraInfo`_ message. + :returns: ``sensor_msgs/CameraInfo`` message. :raises: :exc:`CameraInfoMissingError` if camera info not up to date. @@ -281,7 +290,7 @@ def _loadCalibration(self, url, cname): This method updates self.camera_info, if possible, based on the url and cname parameters. An empty or non-existent calibration is *not* considered an error, a null - `sensor_msgs/CameraInfo`_ being provided in that case. + ``sensor_msgs/CameraInfo`` being provided in that case. :param url: Uniform Resource Locator for calibration data. :param cname: Camera name. @@ -319,7 +328,7 @@ def loadCameraInfo(self): This method updates camera_info, if possible, based on the currently-configured URL and camera name. An empty or non-existent calibration is *not* considered an error; a null - `sensor_msgs/CameraInfo`_ being provided in that case. + ``sensor_msgs/CameraInfo`` being provided in that case. :raises: :exc:`IOError` if an existing calibration is unreadable. @@ -376,9 +385,9 @@ def setURL(self, url): """ Set the calibration URL. - :param cname: camera name to use for saving calibration data + :param url: new calibration URL to use. - :returns: True if new name has valid syntax. + :returns: True if the URL has valid syntax. :post: URL updated, if valid. A new value may change the camera_info, so it will have to be reloaded before @@ -425,11 +434,10 @@ def genCameraName(from_string): def getPackageFileName(url): """ - Get file name corresponding to a `package:` URL. - - `param url` fully-resolved Uniform Resource Locator - `returns` file name if package found, "" otherwise + Get the filesystem path corresponding to a ``package:`` URL. + :param url: Fully-resolved ``package://`` Uniform Resource Locator. + :returns: Absolute file path if the package is found, empty string otherwise. """ # Scan URL from after "package://" until next '/' and extract # package name. The parseURL() already checked that it's present. @@ -455,14 +463,14 @@ def loadCalibrationFile(filename, cname): """ Load calibration data from a file. - This function returns a `sensor_msgs/CameraInfo`_ message, based + This function returns a ``sensor_msgs/CameraInfo`` message, based on the filename parameter. An empty or non-existent file is *not* considered an error; a null CameraInfo being provided in that case. :param filename: location of CameraInfo to read :param cname: Camera name. - :returns: `sensor_msgs/CameraInfo`_ message containing calibration, + :returns: ``sensor_msgs/CameraInfo`` message containing calibration, if file readable; null calibration message otherwise. :raises: :exc:`IOError` if an existing calibration file is unreadable. @@ -499,13 +507,11 @@ def loadCalibrationFile(filename, cname): def parseURL(url): """ - Parse calibration Uniform Resource Locator. - - `param url`: string to parse - `returns` URL type code - - `note`: Unsupported URL types have codes >= URL_invalid. + Parse a calibration URL and return its type code. + :param url: URL string to classify. + :returns: One of ``URL_empty``, ``URL_file``, ``URL_package``, or + ``URL_invalid`` (any value >= ``URL_invalid`` is unsupported). """ if not url: return URL_empty @@ -591,7 +597,7 @@ def saveCalibration(new_info, url, cname): This function writes new calibration information to the location defined by the url and cname parameters, if possible. - :param new_info: `sensor_msgs/CameraInfo`_ to save. + :param new_info: ``sensor_msgs/CameraInfo`` to save. :param url: Uniform Resource Locator for calibration data (if empty use file://${ROS_HOME}/camera_info/${NAME}.yaml). :param cname: Camera name. @@ -638,7 +644,7 @@ def saveCalibrationFile(ci, filename, cname): This function writes the new calibration information to a YAML file, if possible. - :param ci: `sensor_msgs/CameraInfo`_ to save. + :param ci: ``sensor_msgs/CameraInfo`` to save. :param filename: local file to store data. :param cname: Camera name. :returns: True if able to save the data. diff --git a/camera_info_manager_py/doc/api.rst b/camera_info_manager_py/doc/api.rst new file mode 100644 index 00000000..ea3dff31 --- /dev/null +++ b/camera_info_manager_py/doc/api.rst @@ -0,0 +1,27 @@ +API Reference +============= + +Core classes and utilities (module :doc:`/camera_info_manager.camera_info_manager`): + +- :class:`camera_info_manager.camera_info_manager.CameraInfoManager` — + primary class for loading, saving and serving calibration data. +- :class:`camera_info_manager.camera_info_manager.CameraInfoError` — + base exception raised on calibration errors. +- :class:`camera_info_manager.camera_info_manager.CameraInfoMissingError` — + raised when :py:meth:`~camera_info_manager.camera_info_manager.CameraInfoManager.getCameraInfo` + is called before + :py:meth:`~camera_info_manager.camera_info_manager.CameraInfoManager.loadCameraInfo`. +- :func:`camera_info_manager.camera_info_manager.genCameraName` +- :func:`camera_info_manager.camera_info_manager.loadCalibrationFile` +- :func:`camera_info_manager.camera_info_manager.saveCalibration` +- :func:`camera_info_manager.camera_info_manager.resolveURL` +- :func:`camera_info_manager.camera_info_manager.parseURL` + +Zoom camera classes (module :doc:`/camera_info_manager.zoom_camera_info_manager`): + +- :class:`camera_info_manager.zoom_camera_info_manager.ZoomCameraInfoManager` — + abstract base for zoom-aware cameras. +- :class:`camera_info_manager.zoom_camera_info_manager.ApproximateZoomCameraInfoManager` — + computes K matrix from field-of-view bounds. +- :class:`camera_info_manager.zoom_camera_info_manager.InterpolatingZoomCameraInfoManager` — + interpolates between calibrations at multiple zoom levels. diff --git a/camera_info_manager_py/doc/conf.py b/camera_info_manager_py/doc/conf.py new file mode 100644 index 00000000..39941a08 --- /dev/null +++ b/camera_info_manager_py/doc/conf.py @@ -0,0 +1,47 @@ +import inspect +import os +import re +import sys + +# When rosdoc2 runs sphinx-build, it exec's this file from a generated wrapper. +# The wrapper contains: exec(open("/abs/path/to/doc/conf.py").read()) +# Parse that line from the call stack to find the package root and add it to +# sys.path so that autodoc can import camera_info_manager. +try: + import camera_info_manager # noqa: F401 +except ImportError: + for _fi in inspect.stack(): + for _line in (_fi.code_context or []): + _m = re.search(r'exec\(open\("([^"]+)"\)', _line) + if _m: + _pkg_root = os.path.dirname(os.path.dirname(_m.group(1))) + if os.path.isdir(_pkg_root): + sys.path.insert(0, _pkg_root) + break + +project = 'camera_info_manager_py' +copyright = "2024, Open Robotics" +author = "Jack O'Quin, Jose Mastrangelo, Mike Hosmar" + +extensions = [ + 'sphinx.ext.autodoc', + 'sphinx.ext.intersphinx', + 'sphinx.ext.viewcode', + 'myst_parser', +] + +autodoc_member_order = 'bysource' + +source_suffix = { + '.rst': 'restructuredtext', + '.md': 'markdown', +} + +templates_path = ['_templates'] +exclude_patterns = [ + '_build', + 'overview.rst', + 'api.rst', +] + +html_theme = 'sphinx_rtd_theme' diff --git a/camera_info_manager_py/doc/overview.rst b/camera_info_manager_py/doc/overview.rst new file mode 100644 index 00000000..6c84b138 --- /dev/null +++ b/camera_info_manager_py/doc/overview.rst @@ -0,0 +1,105 @@ +Overview +======== + +``camera_info_manager_py`` provides a Python interface for camera calibration +data in ROS 2, mirroring the behaviour of the C++ ``camera_info_manager`` +package. It is intended for camera drivers written in Python that need to: + +- Load ``sensor_msgs/msg/CameraInfo`` from a calibration file on disk. +- Save new calibration data received via the ``set_camera_info`` service. +- Serve the current calibration to the rest of the system. + +Module Structure +---------------- + +.. code-block:: none + + camera_info_manager/ + ├── camera_info_manager.py ← CameraInfoManager + utility functions + └── zoom_camera_info_manager.py ← ZoomCameraInfoManager and subclasses + +``camera_info_manager.py`` +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The main module defines: + +- :class:`~camera_info_manager.CameraInfoManager` — the primary class for + non-zoom cameras. +- :class:`~camera_info_manager.CameraInfoError` — base exception. +- :class:`~camera_info_manager.CameraInfoMissingError` — raised when + ``getCameraInfo()`` / ``isCalibrated()`` is called before + ``loadCameraInfo()``. +- Utility functions: :func:`~camera_info_manager.genCameraName`, + :func:`~camera_info_manager.loadCalibrationFile`, + :func:`~camera_info_manager.saveCalibration`, + :func:`~camera_info_manager.resolveURL`, :func:`~camera_info_manager.parseURL`. + +``zoom_camera_info_manager.py`` +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Provides zoom-aware subclasses: + +- :class:`~camera_info_manager.zoom_camera_info_manager.ZoomCameraInfoManager` + — abstract base for zoom cameras. +- :class:`~camera_info_manager.zoom_camera_info_manager.ApproximateZoomCameraInfoManager` + — computes K matrix from field-of-view bounds. +- :class:`~camera_info_manager.zoom_camera_info_manager.InterpolatingZoomCameraInfoManager` + — interpolates between calibrations taken at multiple zoom levels. + +Calibration URL +--------------- + +The location for loading and saving calibration files is given as a URL. +Supported schemes: + +``file://`` + Absolute filesystem path. + + .. code-block:: none + + file:///home/user/calibrations/camera.yaml + +``package://`` + Path relative to the share directory of a ROS 2 package. + + .. code-block:: none + + package://my_driver/config/camera.yaml + +URL substitution variables (resolved in one pass): + +- ``${NAME}`` — the current camera name. +- ``${ROS_HOME}`` — ``$ROS_HOME`` env var, or ``~/.ros`` if unset. + +Default URL when none is provided: + +.. code-block:: none + + file://${ROS_HOME}/camera_info/${NAME}.yaml + +Explicit Loading +---------------- + +Unlike the C++ ``camera_info_manager``, this Python implementation performs +no lazy loading. You **must** call :py:meth:`~camera_info_manager.CameraInfoManager.loadCameraInfo` +before calling :py:meth:`~camera_info_manager.CameraInfoManager.getCameraInfo` +or :py:meth:`~camera_info_manager.CameraInfoManager.isCalibrated`. +Calling either without a prior ``loadCameraInfo()`` raises +:class:`~camera_info_manager.CameraInfoMissingError`. + +ROS 2 Service +------------- + +The constructor automatically advertises ``set_camera_info`` relative to the +node's namespace (or under an optional custom namespace prefix). Calibration +tools can write new calibration data to the running driver via this service; +the manager saves it to the configured URL and updates the in-memory +``CameraInfo``. + +Differences from C++ camera_info_manager +----------------------------------------- + +- **No lazy loading**: ``loadCameraInfo()`` must be called explicitly. +- **No lifecycle support** in the base class constructor (pass node interfaces + manually if needed). +- **Zoom camera classes** exist only in Python (no C++ equivalent yet). diff --git a/camera_info_manager_py/package.xml b/camera_info_manager_py/package.xml index b0b034f3..3bf02cc6 100644 --- a/camera_info_manager_py/package.xml +++ b/camera_info_manager_py/package.xml @@ -5,12 +5,7 @@ camera_info_manager_py 6.4.7 - - Python interface for camera calibration information. - - This ROS package provides a CameraInfo interface for Python camera - drivers similar to the C++ camera_info_manager package. - + Python interface for camera calibration information, providing CameraInfo support for camera drivers written in Python. Jose Mastrangelo Mike Hosmar BSD-3-Clause @@ -33,6 +28,7 @@ ament_python + rosdoc2.yaml diff --git a/camera_info_manager_py/rosdoc2.yaml b/camera_info_manager_py/rosdoc2.yaml new file mode 100644 index 00000000..8a0a1bdc --- /dev/null +++ b/camera_info_manager_py/rosdoc2.yaml @@ -0,0 +1,19 @@ +## Default configuration, generated by rosdoc2. + +## This 'attic section' self-documents this file's type and version. +type: 'rosdoc2 config' +version: 1 + +--- + +settings: + ## Path (relative to package.xml) to the Python source directory. + ## rosdoc2 adds its parent to sys.path so that 'import camera_info_manager' works. + python_source: camera_info_manager + +builders: + - sphinx: { + name: 'camera_info_manager_py', + sphinx_sourcedir: doc, + output_dir: '' + }