From e7d5e51e2e4e683da301d6e7c5c90a20bec69a47 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 25 Nov 2024 18:34:27 +0100 Subject: [PATCH] Add `camera_info_manager_py` (#335) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero (cherry picked from commit d09c82cfb5558d253de99317a4d7d5fb61867b03) --- camera_info_manager_py/CHANGELOG.rst | 65 ++ camera_info_manager_py/CMakeLists.txt.bak | 34 + camera_info_manager_py/CONTRIBUTING.md | 3 + camera_info_manager_py/LICENSE | 30 + camera_info_manager_py/README.rst | 16 + .../camera_info_manager.rst | 5 + .../camera_info_manager/__init__.py | 35 + .../camera_info_manager.py | 680 ++++++++++++++++++ .../zoom_camera_info_manager.py | 377 ++++++++++ camera_info_manager_py/conf.py | 302 ++++++++ camera_info_manager_py/flake8.ini | 7 + camera_info_manager_py/index.rst | 17 + camera_info_manager_py/package.xml | 39 + .../resource/camera_info_manager_py | 0 camera_info_manager_py/rosdoc.yaml | 4 + camera_info_manager_py/setup.py | 29 + camera_info_manager_py/test/test_copyright.py | 23 + camera_info_manager_py/test/test_flake8.py | 25 + 18 files changed, 1691 insertions(+) create mode 100644 camera_info_manager_py/CHANGELOG.rst create mode 100644 camera_info_manager_py/CMakeLists.txt.bak create mode 100644 camera_info_manager_py/CONTRIBUTING.md create mode 100644 camera_info_manager_py/LICENSE create mode 100644 camera_info_manager_py/README.rst create mode 100644 camera_info_manager_py/camera_info_manager.rst create mode 100644 camera_info_manager_py/camera_info_manager/__init__.py create mode 100644 camera_info_manager_py/camera_info_manager/camera_info_manager.py create mode 100644 camera_info_manager_py/camera_info_manager/zoom_camera_info_manager.py create mode 100644 camera_info_manager_py/conf.py create mode 100644 camera_info_manager_py/flake8.ini create mode 100644 camera_info_manager_py/index.rst create mode 100644 camera_info_manager_py/package.xml create mode 100644 camera_info_manager_py/resource/camera_info_manager_py create mode 100644 camera_info_manager_py/rosdoc.yaml create mode 100644 camera_info_manager_py/setup.py create mode 100644 camera_info_manager_py/test/test_copyright.py create mode 100644 camera_info_manager_py/test/test_flake8.py diff --git a/camera_info_manager_py/CHANGELOG.rst b/camera_info_manager_py/CHANGELOG.rst new file mode 100644 index 00000000..dc04cb1d --- /dev/null +++ b/camera_info_manager_py/CHANGELOG.rst @@ -0,0 +1,65 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package camera_info_manager_py +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +6.0.1 (2024-11-22) +------------------ +* Bump package version to synchronize with image_common +* Contributors: Chris Iverach-Brereton + +1.0.0 (2024-05-16) +------------------ +* Ros2 (`#2 `_) + * Run magic converter + * Ament_python package + * Fix some imports + * Remove references to cpp camera info manager. + Disable tests + * Linting + * Fully Remove old tests + * Add lint tests + * Final tests + * Remove pep257 from depends +* Contributors: Michael Hosmar + +0.3.1 (2021-11-18) +------------------ +* changelog +* Contributors: José Mastrangelo + +0.3.0 (2021-11-18) +------------------ +* added CPR maintainer +* Release to Melodic and Noetic +* Contributors: Jack O'Quin, José Mastrangelo, Lucas Walter, Martin Pecka + +0.2.3 (2014-05-13) +------------------ +* Only use rostest when testing enabled, thanks to Lukas Bulwahn. +* Move repository to ros-perception. +* Contributors: Jack O'Quin, Lukas Bulwahn + +0.2.2 (2013-07-25) +------------------ +* Add namespace parameter to constructor, so a driver can handle multiple cameras. Enhancement thanks to Martin Llofriu. +* Make unit tests conditional on ``CATKIN_ENABLE_TESTING``. +* Release to Groovy and Hydro. +* Contributors: Jack O'Quin, mllofriu + +0.2.1 (2013-04-14) +------------------ +* Set null calibration even when URL invalid (#7). +* Release to Groovy and Hydro. +* Contributors: Jack O'Quin + +0.2.0 (2013-03-28) +------------------ +* Convert to catkin. +* Remove roslib dependency. +* Release to Groovy and Hydro. +* Contributors: Jack O'Quin + +0.1.0 (2012-12-05) +------------------ +* Initial Python camera_info_manager release to Fuerte. +* Contributors: Jack O'Quin diff --git a/camera_info_manager_py/CMakeLists.txt.bak b/camera_info_manager_py/CMakeLists.txt.bak new file mode 100644 index 00000000..77a04e33 --- /dev/null +++ b/camera_info_manager_py/CMakeLists.txt.bak @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.8) +project(camera_info_manager_py) +find_package(ament_cmake REQUIRED) +find_package(rclpy REQUIRED) +find_package(sensor_msgs REQUIRED) + +include_directories(include) + +ament_export_dependencies(sensor_msgs) +ament_package() + +catkin_install_python( + PROGRAMS + src/camera_info_manager/camera_info_manager.py + src/camera_info_manager/zoom_camera_info_manager.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +# Unit test uses nose, but needs rostest to create a ROS environment. +if (BUILD_TESTING) + find_package(roscpp REQUIRED) + find_package(camera_info_manager REQUIRED) + + find_package(rostest REQUIRED) + add_executable(generate_camera_info + tests/generate_camera_info.cpp + ) + # TODO(lucasw) Can the roscpp and camera_info_manager dependencies + # get added to catkin_LIBRARIES somehow? + target_link_libraries(generate_camera_info ${catkin_LIBRARIES} + ${roscpp_LIBRARIES} ${camera_info_manager_LIBRARIES}) + + add_rostest(tests/load_cpp_camera_info.test DEPENDENCIES generate_camera_info) + add_rostest(tests/unit_test.test) +endif(CATKIN_ENABLE_TESTING) diff --git a/camera_info_manager_py/CONTRIBUTING.md b/camera_info_manager_py/CONTRIBUTING.md new file mode 100644 index 00000000..5e190bdc --- /dev/null +++ b/camera_info_manager_py/CONTRIBUTING.md @@ -0,0 +1,3 @@ +Any contribution that you make to this repository will +be under the BSD license 2.0, as dictated by that +[license](https://opensource.org/licenses/BSD-3-Clause). diff --git a/camera_info_manager_py/LICENSE b/camera_info_manager_py/LICENSE new file mode 100644 index 00000000..24d43da7 --- /dev/null +++ b/camera_info_manager_py/LICENSE @@ -0,0 +1,30 @@ +All rights reserved. + +Software License Agreement (BSD License 2.0) + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/camera_info_manager_py/README.rst b/camera_info_manager_py/README.rst new file mode 100644 index 00000000..a313691e --- /dev/null +++ b/camera_info_manager_py/README.rst @@ -0,0 +1,16 @@ +Overview +======== + +This ROS_ package defines a Python camera_info_manager interface, +providing `sensor_msgs/CameraInfo`_ support for camera drivers written +in Python. + +ROS wiki documentation: `camera_info_manager_py`_. + +This interface is similar to that of the C++ `camera_info_manager`_ +package, but not identical. + +.. _ROS: http://ros.org +.. _`sensor_msgs/CameraInfo`: http://ros.org/doc/api/sensor_msgs/html/msg/CameraInfo.html +.. _`camera_info_manager`: http://ros.org/wiki/camera_info_manager +.. _`camera_info_manager_py`: http://ros.org/wiki/camera_info_manager_py diff --git a/camera_info_manager_py/camera_info_manager.rst b/camera_info_manager_py/camera_info_manager.rst new file mode 100644 index 00000000..c52aae5a --- /dev/null +++ b/camera_info_manager_py/camera_info_manager.rst @@ -0,0 +1,5 @@ +camera_info_manager +------------------- + +.. automodule:: camera_info_manager + :members: diff --git a/camera_info_manager_py/camera_info_manager/__init__.py b/camera_info_manager_py/camera_info_manager/__init__.py new file mode 100644 index 00000000..09b0d198 --- /dev/null +++ b/camera_info_manager_py/camera_info_manager/__init__.py @@ -0,0 +1,35 @@ +# Copyright 2024 Clearpath Robotics Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Clearpath Robotics Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +from .camera_info_manager import * # noqa: F401, F403, RUF100 +from .zoom_camera_info_manager import * # noqa: F401, F403, RUF100 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 new file mode 100644 index 00000000..21810778 --- /dev/null +++ b/camera_info_manager_py/camera_info_manager/camera_info_manager.py @@ -0,0 +1,680 @@ +#!/usr/bin/env python +# Copyright 2012, Jack O'Quin +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Python camera_info_manager interface. + +Providing `CameraInfo` support for drivers written in Python. +This is very similar to the + +`C++ camera_info_manager`_ package, but not identical. + +.. _`C++ camera_info_manager`: http://ros.org/wiki/camera_info_manager +.. _`sensor_msgs/CameraInfo`: http://ros.org/doc/api/sensor_msgs/html/msg/CameraInfo.html +.. _`sensor_msgs/SetCameraInfo`: http://ros.org/doc/api/sensor_msgs/html/srv/SetCameraInfo.html + +""" + +import errno +import locale +import os +from pathlib import Path + +from ament_index_python import get_package_share_directory +from ament_index_python import PackageNotFoundError +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo +from sensor_msgs.srv import SetCameraInfo +import yaml + +default_camera_info_url = 'file://${ROS_HOME}/camera_info/${NAME}.yaml' +# parseURL() type codes: +URL_empty = 0 # empty string +URL_file = 1 # file: +URL_package = 2 # package: +URL_invalid = 3 # anything >= is invalid + + +class CameraInfoError(Exception): + """Base class for exceptions in this module. + + ..exception: CameraInfoError + """ + + +class CameraInfoMissingError(CameraInfoError): + """Exception raised when CameraInfo has not been loaded. + + ..exception: CameraInfoMissingError + """ + + +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`_ + data. + + :param cname: camera name. + :param url: Uniform Resource Locator for camera calibration data. + :param namespace: Optional ROS namespace prefix for the service + name. If a namespace is specified, the '/' separator + required between it and ``set_camera_info`` will be + supplied automatically. + + .. describe:: str(camera_info_manager_obj) + + :returns: String representation of :class:`CameraInfoManager` object. + + **ROS Service** + + - 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 *must* invoke `rospy.spin()` in some thread, so + :class:`CameraInfoManager` gets called to handle arriving service + requests. + + If a driver handles multiple cameras, it should use the + ``namespace`` parameter to declare separate + :class:`CameraInfoManager` instances for each one, as in this + stereo example:: + + left_ci = CameraInfoManager(cname='left_camera', namespace='left') + right_ci = CameraInfoManager(cname='right_camera', namespace='right') + + **Camera Name** + + The device driver sets a camera name via the + :class:`CameraInfoManager` constructor or the + :py:meth:`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. + + The camera name can be resolved as part of the URL, allowing + direct access to device-specific calibration information. + + **Uniform Resource Locator** + + The location for getting and saving calibration data is expressed + by Uniform Resource Locator. The driver defines a URL via the + :class:`CameraInfoManager` constructor or the :py:meth:`setURL` + method. Many drivers provide a `~camera_info_url` parameter so + users may customize this URL, but that is handled outside this + class. + + Camera calibration information is stored in YAML format. + + Example URL syntax: + + - `file:///full/path/to/local/file.yaml` + - `package://camera_info_manager_py/tests/test_calibration.yaml` + - `package://ros_package_name/calibrations/camera3.yaml` + + 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`. + + 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: + + - `package://my_cameras/calibrations/${NAME}.yaml` + - `file://${ROS_HOME}/camera_info/left_front_camera.yaml` + + The default URL is: + + - `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. + + **Loading Calibration Data** + + 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. + + If the URL or camera name changes, :py:meth:`loadCameraInfo` must + be called again before the data are accessible. + + """ + + def __init__(self, node: Node, cname='camera', url='', namespace=''): + """Call the Constructor.""" + self.node = node + self.cname = cname + self.url = url + self.camera_info = None + + # advertise set_camera_info service + service_name = 'set_camera_info' + if namespace: + service_name = namespace + '/' + service_name + self.node.get_logger().debug(service_name + ' service declared') + self.svc = self.node.create_service(SetCameraInfo, service_name, self.setCameraInfo) + + def __str__(self): + """Return string representation of CameraInfoManager. + + :returns: Return string representation of :class:CameraInfoManager. + """ + return '[' + self.cname + ']' + str(self.utm) + + def getCameraInfo(self): + """Get the current camera calibration. + + The :py:meth:`loadCameraInfo` must have been called since the + last time the camera name or URL changed. + + :returns: `sensor_msgs/CameraInfo`_ message. + + :raises: :exc:`CameraInfoMissingError` if camera info not up + to date. + + """ + if self.camera_info is None: + raise CameraInfoMissingError('Calibration missing, loadCameraInfo() needed.') + return self.camera_info + + def getCameraName(self): + """Get the current camera name. + + :returns: camera name string + """ + return self.cname + + def getURL(self): + """Get the current calibration URL. + + :returns: URL string without variable expansion. + """ + return self.url + + def isCalibrated(self): + """Is the current CameraInfo calibrated?. + + The :py:meth:`loadCameraInfo` must have been called since the + last time the camera name or URL changed. + + :returns: True if camera calibration exists; + False for null calibration. + + :raises: :exc:`CameraInfoMissingError` if camera info not up + to date. + + """ + if self.camera_info is None: + raise CameraInfoMissingError('Calibration missing, ' + 'loadCameraInfo() needed.') + return self.camera_info.K[0] != 0.0 + + def _loadCalibration(self, url, cname): + """Load calibration data (if any available). + + 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. + + :param url: Uniform Resource Locator for calibration data. + :param cname: Camera name. + + :raises: :exc:`IOError` if an existing calibration is unreadable. + :raises: :exc:`CameraInfoMissingError` if a `package:` URL is + inaccessible. + """ + resolved_url = resolveURL(url, cname) + url_type = parseURL(resolved_url) + + if url_type == URL_empty: + self._loadCalibration(default_camera_info_url, cname) + return + + self.node.get_logger().info('camera calibration URL: ' + resolved_url) + + if url_type == URL_file: + self.camera_info = loadCalibrationFile(resolved_url[7:], cname) + + elif url_type == URL_package: + filename = getPackageFileName(resolved_url) + if not filename: # package not resolved + raise CameraInfoMissingError('Calibration package missing.') + self.camera_info = loadCalibrationFile(filename, cname) + + else: + self.node.get_logger().error('Invalid camera calibration URL: ' + resolved_url) + self.camera_info = CameraInfo() + + def loadCameraInfo(self): + """Load currently configured calibration data (if any). + + 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. + + :raises: :exc:`IOError` if an existing calibration is unreadable. + + """ + self._loadCalibration(self.url, self.cname) + + def setCameraInfo(self, req): + """Set camera info request callback. + + :param req: SetCameraInfo request message. + :returns: SetCameraInfo response message, success is True if + message handled. + + :post: camera_info updated, can be used immediately without + reloading. + """ + self.node.get_logger().debug('SetCameraInfo received for ' + self.cname) + self.camera_info = req.camera_info + rsp = SetCameraInfo.Response() + rsp.success = saveCalibration(req.camera_info, self.url, self.cname) + if not rsp.success: + rsp.status_message = 'Error storing camera calibration.' + return rsp + + def setCameraName(self, cname): + """Set a new camera name. + + :param cname: camera name to use for saving calibration data + + :returns: True if new name has valid syntax; valid names + contain only alphabetic, numeric, or '_' characters. + + :post: camera name updated, if valid. A new name may affect + the URL, so camera_info will have to be reloaded before + being used again. + + """ + # validate name + if not cname: + return False # name may not be empty + for ch in cname: + if not ch.isalnum() and ch != '_': + return False # invalid character + + # name is valid, use it + if self.cname != cname: + self.cname = cname + self.camera_info = None # missing if name changed + return True + + def setURL(self, url): + """Set the calibration URL. + + :param cname: camera name to use for saving calibration data + + :returns: True if new name has valid syntax. + + :post: URL updated, if valid. A new value may change the + camera_info, so it will have to be reloaded before + being used again. + + """ + if parseURL(resolveURL(url, self.cname)) >= URL_invalid: + return False # syntax error + + # URL looks valid, so use it + if self.url != url: + self.url = url + self.camera_info = None # missing if URL changed + return True + + +# related utility functions + + +def genCameraName(from_string): + """Generate a valid camera name. + + Valid names contain only alphabetic, numeric, or '_' + characters. All invalid characters in from_string are replaced + by an '_'. + + :param from_string: string from which to base camera name. + + :returns: a valid camera name based on from_string. + + """ + if not from_string: + return '_' # name may not be empty + + retval = '' + for i in range(len(from_string)): + if not from_string[i].isalnum() and from_string[i] != '_': + retval += '_' + else: + retval += from_string[i] + return retval + + +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 + + """ + # Scan URL from after "package://" until next '/' and extract + # package name. The parseURL() already checked that it's present. + prefix_len = len('package://') + rest = url.find('/', prefix_len) + package = url[prefix_len:rest] + + # Look up the ROS package path name. + pkgPath = '' + try: + pkgPath = get_package_share_directory(package) + pkgPath += url[rest:] + + except PackageNotFoundError: + rclpy.get_logger('camera_info_manager').warning( + 'unknown package: ' + package + ' (ignored)' + ) + + return pkgPath + + +def loadCalibrationFile(filename, cname): + """Load calibration data from a file. + + 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, + if file readable; null calibration message otherwise. + :raises: :exc:`IOError` if an existing calibration file is unreadable. + + """ + ci = CameraInfo() + try: + with Path(filename).open(encoding=locale.getpreferredencoding(False)) as f: + calib = yaml.safe_load(f) + if calib is not None: + if calib['camera_name'] != cname: + rclpy.get_logger('camera_info_manager').warn( + '[' + + cname + + '] does not match name ' + + calib['camera_name'] + + ' in file ' + + filename + ) + + # fill in CameraInfo fields + ci.width = calib['image_width'] + ci.height = calib['image_height'] + ci.distortion_model = calib['distortion_model'] + ci.D = calib['distortion_coefficients']['data'] + ci.K = calib['camera_matrix']['data'] + ci.R = calib['rectification_matrix']['data'] + ci.P = calib['projection_matrix']['data'] + + except OSError: # OK if file did not exist + pass + + return ci + + +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. + + """ + if not url: + return URL_empty + + if url[0:8].upper() == 'FILE:///': + return URL_file + + if url[0:10].upper() == 'PACKAGE://': + # look for a '/' following the package name, make sure it is + # there, the name is not empty, and something follows it + + rest = url.find('/', 10) + if rest < len(url) - 1 and rest >= 0: + return URL_package + + return URL_invalid + + +def resolveURL(url, cname): + """Resolve substitution strings in Uniform Resource Locator. + + :param url: URL to resolve, which may include `${...}` + substitution variables. + :param cname: camera name for resolving `${NAME}` variables. + + :returns: a copy of the URL with any variable information resolved. + + """ + resolved = '' # resolved URL to return + rest = 0 # index of remaining string to parse + + while True: + # find the next '$' in the URL string + dollar = url.find('$', rest) + + if dollar == -1: # no more '$'s there? + resolved += url[rest:] + return resolved + + # copy characters up to the next '$' + resolved += url[rest:dollar] + + if url[dollar + 1 : dollar + 2] != '{': + # no '{' follows, so keep the '$' + resolved += '$' + + elif url[dollar + 1 : dollar + 7] == '{NAME}': + # substitute camera name + resolved += cname + dollar += 6 + + elif url[dollar + 1 : dollar + 11] == '{ROS_HOME}': + # substitute $ROS_HOME + ros_home = os.environ.get('ROS_HOME') + if ros_home is None: + ros_home = os.environ.get('HOME') + if ros_home is None: + rclpy.get_logger('camera_info_manager').warn( + '[CameraInfoManager]' + ' unable to resolve ${ROS_HOME}' + ) + ros_home = '${ROS_HOME}' # retain it unresolved + else: + ros_home += '/.ros' + resolved += ros_home + dollar += 10 + + else: + # not a valid substitution variable + rclpy.get_logger('camera_info_manager').warn( + '[CameraInfoManager] invalid URL substitution (not resolved): ' + url + ) + resolved += '$' # keep the bogus '$' + + # look for next '$' + rest = dollar + 1 + + +def saveCalibration(new_info, url, cname): + """Save calibration data. + + 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 url: Uniform Resource Locator for calibration data (if + empty use file://${ROS_HOME}/camera_info/${NAME}.yaml). + :param cname: Camera name. + :returns: True if able to save the data. + """ + success = False + resolved_url = resolveURL(url, cname) + url_type = parseURL(resolved_url) + + if url_type == URL_empty: + return saveCalibration(new_info, default_camera_info_url, cname) + + rclpy.get_logger('camera_info_manager').info( + 'writing calibration data to URL: ' + resolved_url + ) + + if url_type == URL_file: + success = saveCalibrationFile(new_info, resolved_url[7:], cname) + + elif url_type == URL_package: + filename = getPackageFileName(resolved_url) + if not filename: # package not resolved + rclpy.get_logger('camera_info_manager').error( + 'Calibration package missing: ' + resolved_url + ' (ignored)' + ) + # treat it like an empty URL + success = saveCalibration(new_info, default_camera_info_url, cname) + else: + success = saveCalibrationFile(new_info, filename, cname) + + else: + rclpy.get_logger('camera_info_manager').error( + 'Invalid camera calibration URL: ' + resolved_url + ) + # treat it like an empty URL + success = saveCalibration(new_info, default_camera_info_url, cname) + return success + + +def saveCalibrationFile(ci, filename, cname): + """Save calibration data to a YAML file. + + This function writes the new calibration information to a YAML + file, if possible. + + :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. + """ + # make calibration dictionary from CameraInfo fields and camera name + calib = { + 'image_width': ci.width, + 'image_height': ci.height, + 'camera_name': cname, + 'distortion_model': ci.distortion_model, + 'distortion_coefficients': {'data': ci.D, 'rows': 1, 'cols': len(ci.D)}, + 'camera_matrix': {'data': ci.K, 'rows': 3, 'cols': 3}, + 'rectification_matrix': {'data': ci.R, 'rows': 3, 'cols': 3}, + 'projection_matrix': {'data': ci.P, 'rows': 3, 'cols': 4}, + } + + # make sure the directory exists and the file is writable + try: + with Path(filename).open('w', encoding=locale.getpreferredencoding(False)) as f: + try: + yaml.safe_dump(calib, f) + return True + except OSError: + return False # fail if unable to write file + except OSError as e: + if e.errno in {errno.EACCES, errno.EPERM}: + rclpy.get_logger('camera_info_manager').error('file [' + filename + '] not accessible') + return False # unable to write this file + if e.errno in {errno.ENOENT}: + # Find last slash in the name. The URL parser ensures + # there is at least one '/', at the beginning. + last_slash = filename.rfind('/') + if last_slash < 0: + rclpy.get_logger('camera_info_manager').error( + 'filename [' + filename + "] has no '/'" + ) + return False # not a valid URL + + # try to create the directory and all its parents + dirname = filename[0 : last_slash + 1] + try: + Path(dirname).mkdir(parents=True) + except OSError: + rclpy.get_logger('camera_info_manager').error( + 'unable to create path to directory [' + dirname + ']' + ) + return False + + # try again to create the file + with Path(filename).open('w', encoding=locale.getpreferredencoding(False)) as f: + try: + yaml.safe_dump(calib, f) + return True + + except OSError: + rclpy.get_logger('camera_info_manager').error( + 'file [' + filename + '] not accessible' + ) + return False # fail if unable to write file diff --git a/camera_info_manager_py/camera_info_manager/zoom_camera_info_manager.py b/camera_info_manager_py/camera_info_manager/zoom_camera_info_manager.py new file mode 100644 index 00000000..f41e68f4 --- /dev/null +++ b/camera_info_manager_py/camera_info_manager/zoom_camera_info_manager.py @@ -0,0 +1,377 @@ +#!/usr/bin/env python +# Copyright 2016, Martin Pecka +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Martin Pecka nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +""" +`CameraInfo` support for drivers of zoom cameras written in Python. + +A similar C++ API does not exist yet. +""" +# enable some python3 compatibility options: + +from copy import deepcopy +from math import radians, tan + +from sensor_msgs.msg import CameraInfo + +from camera_info_manager import ( + CameraInfoError, + CameraInfoManager, + CameraInfoMissingError, + getPackageFileName, + loadCalibrationFile, + parseURL, + resolveURL, + URL_empty, + URL_file, + URL_package, +) + + +class ZoomCameraInfoManager(CameraInfoManager): + """ + :class:`ZoomCameraInfoManager` provides ROS CameraInfo support for Python zoom camera drivers. + + This is a base class for all zoom cameras (since there are several + ways to implement the changing camera infos). + + No standardized calibration package/procedure has been released yet, + but at each specific class' docstring contains some hints on how to + calibrate a zoom camera. + """ + + def __init__(self, min_zoom, max_zoom, cname='camera', url='', namespace=''): + """ + Construct the manager. + + :param int min_zoom: The minimum zoom level. + The zoom values should linearly affect the focal distance. + :param int max_zoom: The maximum zoom level. + The zoom values should linearly affect the focal distance. + :param string cname: camera name. + :param string url: Uniform Resource Locator for camera calibration data. + This should point to the minimum zoom calibration file. + :param string namespace: Optional ROS namespace prefix for the service name. + If a namespace is specified, the '/' separator required between it + and ``set_camera_info`` will be supplied automatically. + """ + CameraInfoManager.__init__(self, cname, url, namespace) + + self._min_zoom = min_zoom + self._max_zoom = max_zoom + self._zoom = min_zoom + + def _update_camera_info(self): + """Update the camera info after zoom or calibration has changed.""" + raise NotImplementedError() + + def set_zoom(self, zoom): + """ + Set zoom to the given level and update the camera info. + + :param int zoom: The zoom level. + + :raises: :exc:`CameraInfoError` if + the zoom level is outside the bounds given in constructor. + """ + if zoom < self._min_zoom or zoom > self._max_zoom: + raise CameraInfoError('Zoom got outside the allowed range') + + self._zoom = zoom + + self._update_camera_info() + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_val, exc_tb): + self.svc.shutdown() + + +class ApproximateZoomCameraInfoManager(ZoomCameraInfoManager): + """ + A zoom camera info manager. + + A zoom camera info manager implementation that only approximates the K matrix + by computing it from specified minimum and maximum field of view (FOV). This + will probably never be very precise, but may work well enough for some cameras. + + Only the K matrix is going to be computed. All other parts of the :class:`CameraInfo` + message are either empty or copied from the loaded calibration file (if provided). + + Especially, the distortion coefficients should also decrease with increasing zoom + level, but the relation is unclear. + + Hints for calibration: + You should find the minimum and maximum FOV (Field Of View) of your camera in its + documentation (or the documentation of the lens). + The FOV passed to constructor is the horizontal FOV; vertical FOV is derived from + the horizontal FOV and image aspect ratio. You can also measure the FOV physically + by shooting a tape measure and using a bit of trigonometry. + Ideally, you should calibrate the camera for the lowest and highest zoom levels, + and then compare, if the K matrix returned by this manager is similar. If it is not, + just play with the minimum/maximum FOV, until you are close enough at both ends. + """ + + def __init__( + self, + min_fov, + max_fov, + initial_image_width, + initial_image_height, + min_zoom, + max_zoom, + cname='camera', + url='', + namespace='', + ): + """ + Construct the manager. + + :param float min_fov: The minimum horizontal field of view. Corresponds to + maximum zoom level. + :param float max_fov: The maximum horizontal field of view. Corresponds to + minimum zoom level. + :param int initial_image_width: Horizontal resolution of the image. + :param int initial_image_height: Vertical resolution of the image. + :param int min_zoom: The minimum zoom level. The zoom values should linearly + affect the focal distance. + :param int max_zoom: The maximum zoom level. The zoom values should linearly + affect the focal distance. + :param string cname: camera name. + :param string url: Uniform Resource Locator for camera calibration data. This + should point to the minimum zoom calibration file. + :param string namespace: Optional ROS namespace prefix for the service name. + If a namespace is specified, the '/' separator required between it and + ``set_camera_info`` will be supplied automatically. + """ + ZoomCameraInfoManager.__init__(self, min_zoom, max_zoom, cname, url, namespace) + + self._min_fov = min_fov + self._max_fov = max_fov + + self._image_width = initial_image_width + self._image_height = initial_image_height + + self._loaded_camera_info = None + """Camera info loaded from the calibration URL.""" + + def loadCameraInfo(self): + CameraInfoManager.loadCameraInfo(self) + + self._loaded_camera_info = deepcopy(self.camera_info) + + def set_resolution(self, width, height): + """ + Set resolution of the image plane and updates the camera info. + + :param int width: Width of the image in px. + :param int height: Height of the image in px. + """ + self._image_width = width + self._image_height = height + + self._update_camera_info() + + def _update_camera_info(self): + if self._loaded_camera_info is None: + return + + self.camera_info = deepcopy(self._loaded_camera_info) + self.camera_info.K[8] = 1.0 + + aspect_ratio = float(self._image_height) / self._image_width + zoom_percentage = float(self._zoom - self._min_zoom) / float( + self._max_zoom - self._min_zoom + ) + + horizontal_fov = radians( + (self._max_fov - self._min_fov) * (1 - zoom_percentage) + self._min_fov + ) + horizontal_focal_length_in_px = self._image_width / (2 * tan(horizontal_fov / 2)) + self.camera_info.K[0] = horizontal_focal_length_in_px + + vertical_fov = horizontal_fov * aspect_ratio + vertical_focal_length_in_px = self._image_height / (2 * tan(vertical_fov / 2)) + self.camera_info.K[4] = vertical_focal_length_in_px + + self.camera_info.width = self._image_width + self.camera_info.height = self._image_height + + if self._loaded_camera_info.K[2] != 0.0: + # if a standard calibration is available, just scale the principal point + # offset with the resolution + self.camera_info.K[2] = ( + float(self._image_width) + / self._loaded_camera_info.width + * self._loaded_camera_info.K[2] + ) + self.camera_info.K[5] = ( + float(self._image_height) + / self._loaded_camera_info.height + * self._loaded_camera_info.K[5] + ) + else: + # if no calibration is available, just set the principal point to lie in + # the middle of the image + self.camera_info.K[2] = self._image_width / 2.0 + self.camera_info.K[5] = self._image_height / 2.0 + + +class InterpolatingZoomCameraInfoManager(ZoomCameraInfoManager): + """ + An Interpoated zoom camera info manager. + + Interpolates between several calibrations taken at different zoom levels. + + The calibration results can be as accurate as you need (you increase accuracy + by adding more calibration files). E.g. if the camera has only a few discrete + zoom steps, you can just calibrate all of them and reach the lowest calibration + error possible. + + Hints for calibration: + Use the standard camera calibration package, set a fixed zoom level and perform + the calibration. Copy the calibration YAML file to some other location, change + the zoom level, and calibrate again. Rename all the calibration YAML files, so + that their names correspond to the calibration_url_template you pass to the + constructor. + It is advised to always perform calibration on (at least) the highest and lowest + zoom step. + The last calibration should be on the most-used zoom level, in case some other + code doesn't know this is a zoom camera + """ + + def __init__( + self, calibration_url_template, zoom_levels, cname='camera', url='', namespace='' + ): + """ + Construct the manager. + + :param string calibration_url_template: Template of the URL that contains the + calibration files. The template string should contain a single '%d' + substitution, which will be substituted with zoom level. + :param list zoom_levels: The zoom levels at which the calibration files exist. + The smallest value is automatically taken as the minimum allowed zoom, and + respectively for the largest one. + :param string cname: camera name. + :param string url: Uniform Resource Locator for camera calibration data. This + should point to the minimum zoom calibration file. + :param string namespace: Optional ROS namespace prefix for the service name. + If a namespace is specified, the '/' separator required between it and + ``set_camera_info`` will be supplied automatically. + """ + ZoomCameraInfoManager.__init__( + self, min(zoom_levels), max(zoom_levels), cname, url, namespace + ) + + self._calibration_url_template = calibration_url_template + self._zoom_levels = zoom_levels + + self._camera_infos = None + + def _update_camera_info(self): + if not self.isCalibrated() or self._camera_infos is None: + return + + if self._zoom in list(self._camera_infos.keys()): + self.camera_info = deepcopy(self._camera_infos[self._zoom]) + return + + # find the closest zoom levels at which we have calibration available + closest_higher_zoom = min(z for z in self._zoom_levels if z >= self._zoom) + closest_lower_zoom = max(z for z in self._zoom_levels if z <= self._zoom) + + # compute the weight of the lower and higher calibration files + ratio = float(self._zoom - closest_lower_zoom) / float( + closest_higher_zoom - closest_lower_zoom + ) + + # linearly interpolate all the matrices + self.camera_info = deepcopy(self._camera_infos[closest_lower_zoom]) + self.camera_info.K = [ + (ratio * low + (1 - ratio) * high) + for (low, high) in zip( + self._camera_infos[closest_lower_zoom].K, self._camera_infos[closest_higher_zoom].K + ) + ] + self.camera_info.P = [ + (ratio * low + (1 - ratio) * high) + for (low, high) in zip( + self._camera_infos[closest_lower_zoom].P, self._camera_infos[closest_higher_zoom].P + ) + ] + self.camera_info.R = [ + (ratio * low + (1 - ratio) * high) + for (low, high) in zip( + self._camera_infos[closest_lower_zoom].R, self._camera_infos[closest_higher_zoom].R + ) + ] + self.camera_info.D = [ + (ratio * low + (1 - ratio) * high) + for (low, high) in zip( + self._camera_infos[closest_lower_zoom].D, self._camera_infos[closest_higher_zoom].D + ) + ] + + def loadCameraInfo(self): + CameraInfoManager.loadCameraInfo(self) + + # load all calibration files for all zoom levels + self._camera_infos = {} + for zoom_level in self._zoom_levels: + resolved_url = resolveURL(self._calibration_url_template % zoom_level, self.cname) + url_type = parseURL(resolved_url) + + if url_type == URL_empty: + raise CameraInfoError('Zoom camera cannot use default calibration URLs.') + + self.node.get_logger().info( + 'camera calibration URL for zoom level %d: %s' % (zoom_level, resolved_url) + ) + + if url_type == URL_file: + self._camera_infos[zoom_level] = loadCalibrationFile(resolved_url[7:], self.cname) + + elif url_type == URL_package: + filename = getPackageFileName(resolved_url) + if not filename: # package not resolved + raise CameraInfoMissingError('Calibration package missing.') + self._camera_infos[zoom_level] = loadCalibrationFile(filename, self.cname) + + else: + self.node.get_logger().error('Invalid camera calibration URL: ' + resolved_url) + self._camera_infos[zoom_level] = CameraInfo() + + if len(list(self._camera_infos.keys())) < 2: + raise CameraInfoError( + 'Interpolating zoom camera info manager needs at least two calibrations to exist.' + ) diff --git a/camera_info_manager_py/conf.py b/camera_info_manager_py/conf.py new file mode 100644 index 00000000..5ed8f2c2 --- /dev/null +++ b/camera_info_manager_py/conf.py @@ -0,0 +1,302 @@ +# Copyright 2024 Clearpath Robotics Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Clearpath Robotics Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +# +# camera_info_manager_py documentation build configuration file, created by +# sphinx-quickstart on Wed Apr 25 16:01:20 2012. +# +# This file is execfile()d with the current directory set to its containing dir. +# +# Note that not all possible configuration values are present in this +# autogenerated file. +# +# All configuration values have a default; values that are commented out +# serve to show the default. + +import os +import sys + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. + +sys.path.insert(0, os.path.abspath('./src/camera_info_manager')) + +# -- General configuration ----------------------------------------------------- + +# If your documentation needs a minimal Sphinx version, state it here. +# needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be extensions +# coming with Sphinx (named 'sphinx.ext.*') or your custom ones. +extensions = [ + 'sphinx.ext.autodoc', + 'sphinx.ext.intersphinx', + 'sphinx.ext.todo', + 'sphinx.ext.viewcode', +] + +# List automatically-documented module members in source code order. +autodoc_member_order = 'bysource' + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# The suffix of source filenames. +source_suffix = '.rst' + +# The encoding of source files. +# source_encoding = 'utf-8-sig' + +# The master toctree document. +master_doc = 'index' + +# General information about the project. +project = 'camera_info_manager_py' +# copyright = "2012, Jack O'Quin" + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# +# The short X.Y version. +version = '0.2.0' +# The full version, including alpha/beta/rc tags. +release = '0.2.0' + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +# language = None + +# There are two options for replacing |today|: either, you set today to some +# non-false value, then it is used: +# today = '' +# Else, today_fmt is used as the format for a strftime call. +# today_fmt = '%B %d, %Y' + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +exclude_patterns = [] + +# The reST default role (used for this markup: `text`) to use for all documents. +# default_role = None + +# If true, '()' will be appended to :func: etc. cross-reference text. +# add_function_parentheses = True + +# If true, the current module name will be prepended to all description +# unit titles (such as .. function::). +# add_module_names = True + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +# show_authors = False + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = 'sphinx' + +# A list of ignored prefixes for module index sorting. +# modindex_common_prefix = [] + + +# -- Options for HTML output --------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +html_theme = 'default' + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +# html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +# html_theme_path = [] + +# The name for this set of Sphinx documents. If None, it defaults to +# " v documentation". +# html_title = None + +# A shorter title for the navigation bar. Default is the same as html_title. +# html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +# html_logo = None + +# The name of an image file (within the static path) to use as favicon of the +# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +# html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = [] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +# html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +# html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +# html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +# html_additional_pages = {} + +# If false, no module index is generated. +# html_domain_indices = True + +# If false, no index is generated. +# html_use_index = True + +# If true, the index is split into individual pages for each letter. +# html_split_index = False + +# If true, links to the reST sources are added to the pages. +# html_show_sourcelink = True + +# If true, "Created using Sphinx" is shown in the HTML footer. Default is True. +# html_show_sphinx = True + +# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. +# html_show_copyright = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +# html_use_opensearch = '' + +# This is the file name suffix for HTML files (e.g. ".xhtml"). +# html_file_suffix = None + +# Output file base name for HTML help builder. +htmlhelp_basename = 'camera_info_manager_pydoc' + + +# -- Options for LaTeX output -------------------------------------------------- + +latex_elements = { + # The paper size ('letterpaper' or 'a4paper'). + # 'papersize': 'letterpaper', + # The font size ('10pt', '11pt' or '12pt'). + # 'pointsize': '10pt', + # Additional stuff for the LaTeX preamble. + # 'preamble': '', +} + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, author, documentclass [howto/manual]). +latex_documents = [ + ( + 'index', + 'camera_info_manager_py.tex', + 'ROS Python camera_info_manager support.', + "Jack O'Quin", + 'manual', + ), +] + +# The name of an image file (relative to this directory) to place at the top of +# the title page. +# latex_logo = None + +# For "manual" documents, if this is true, then toplevel headings are parts, +# not chapters. +# latex_use_parts = False + +# If true, show page references after internal links. +# latex_show_pagerefs = False + +# If true, show URL addresses after external links. +# latex_show_urls = False + +# Documents to append as an appendix to all manuals. +# latex_appendices = [] + +# If false, no module index is generated. +# latex_domain_indices = True + + +# -- Options for manual page output -------------------------------------------- + +# One entry per manual page. List of tuples +# (source start file, name, description, authors, manual section). +man_pages = [ + ( + 'index', + 'camera_info_manager_py', + 'ROS Python camera_info_manager support.', + ["Jack O'Quin"], + 1, + ) +] + +# If true, show URL addresses after external links. +# man_show_urls = False + + +# -- Options for Texinfo output ------------------------------------------------ + +# Grouping the document tree into Texinfo files. List of tuples +# (source start file, target name, title, author, +# dir menu entry, description, category) +texinfo_documents = [ + ( + 'index', + 'camera_info_manager_py', + 'camera_info_manager_py Documentation', + "Jack O'Quin", + 'camera_info_manager_py', + 'camera_info_manager ROS interface for Python camera drivers.', + 'Miscellaneous', + ), +] + +# Documents to append as an appendix to all manuals. +# texinfo_appendices = [] + +# If false, no module index is generated. +# texinfo_domain_indices = True + +# How to display URL addresses: 'footnote', 'no', or 'inline'. +# texinfo_show_urls = 'footnote' + + +# Example configuration for intersphinx: refer to the Python standard library. +intersphinx_mapping = {'http://docs.python.org/': None} diff --git a/camera_info_manager_py/flake8.ini b/camera_info_manager_py/flake8.ini new file mode 100644 index 00000000..b9d305f6 --- /dev/null +++ b/camera_info_manager_py/flake8.ini @@ -0,0 +1,7 @@ +[flake8] +extend-ignore = C816,D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404,I202,E203 +import-order-style = google +max-line-length = 99 +show-source = true +statistics = true +application-import-names = camera_info_manager \ No newline at end of file diff --git a/camera_info_manager_py/index.rst b/camera_info_manager_py/index.rst new file mode 100644 index 00000000..ce435ab5 --- /dev/null +++ b/camera_info_manager_py/index.rst @@ -0,0 +1,17 @@ +camera_info_manager: ROS CameraInfo support for Python camera drivers +===================================================================== + +Contents: + +.. toctree:: + :maxdepth: 2 + + README + camera_info_manager + CHANGELOG + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`modindex` diff --git a/camera_info_manager_py/package.xml b/camera_info_manager_py/package.xml new file mode 100644 index 00000000..4a8bfb84 --- /dev/null +++ b/camera_info_manager_py/package.xml @@ -0,0 +1,39 @@ + + + + camera_info_manager_py + 6.0.1 + + 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. + + Jose Mastrangelo + Mike Hosmar + BSD + http://ros.org/wiki/camera_info_manager_py + https://github.com/ros-perception/image_common.git + https://github.com/ros-perception/image_common/issues + + Jack O'Quin + + ament_index_python + + rclpy + sensor_msgs + python3-rospkg + python3-yaml + + ament_copyright + ament_flake8 + python3-pytest + + + + ament_python + + + diff --git a/camera_info_manager_py/resource/camera_info_manager_py b/camera_info_manager_py/resource/camera_info_manager_py new file mode 100644 index 00000000..e69de29b diff --git a/camera_info_manager_py/rosdoc.yaml b/camera_info_manager_py/rosdoc.yaml new file mode 100644 index 00000000..1bda6fbe --- /dev/null +++ b/camera_info_manager_py/rosdoc.yaml @@ -0,0 +1,4 @@ + - builder: sphinx + name: Python API + output_dir: python + diff --git a/camera_info_manager_py/setup.py b/camera_info_manager_py/setup.py new file mode 100644 index 00000000..475fd00a --- /dev/null +++ b/camera_info_manager_py/setup.py @@ -0,0 +1,29 @@ +import os + +from setuptools import setup + +package_name = 'camera_info_manager_py' + +setup( + name=package_name, + version='6.0.1', + packages=['camera_info_manager'], + data_files=[ + # Install marker file in the package index + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + # Include our package.xml file + (os.path.join('share', package_name), ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Jose Mastrangelo', + maintainer_email='jmastrangelo@clearpathrobotics.com', + author="Jack O'Quin", + author_email='jack.oquin@gmail.com', + description='Python interface for camera calibration information.', + long_description='Python interface for camera calibration information. \n\n' + 'This ROS package provides a CameraInfo interface for Python camera\n' + 'drivers similar to the C++ camera_info_manager package.', + license='BSD', + tests_require=['pytest'], +) diff --git a/camera_info_manager_py/test/test_copyright.py b/camera_info_manager_py/test/test_copyright.py new file mode 100644 index 00000000..aa74f3e0 --- /dev/null +++ b/camera_info_manager_py/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.']) + assert rc == 0, 'Found errors' diff --git a/camera_info_manager_py/test/test_flake8.py b/camera_info_manager_py/test/test_flake8.py new file mode 100644 index 00000000..aa116516 --- /dev/null +++ b/camera_info_manager_py/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=['--config', 'flake8.ini', '.']) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors)