From c7f104bd33d840f7f6b8d2270b9a94787fe6973f Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Sat, 3 Feb 2024 21:38:11 -0500 Subject: [PATCH] WIP --- depth_image_proc/doc/changes.rst | 18 +++ depth_image_proc/doc/components.rst | 178 +++++++++++++++++++++++++ depth_image_proc/doc/conf.py | 194 ++++++++++++++++++++++++++++ depth_image_proc/doc/index.rst | 40 ++++++ depth_image_proc/doc/tutorials.rst | 93 +++++++++++++ depth_image_proc/src/register.cpp | 4 +- 6 files changed, 525 insertions(+), 2 deletions(-) create mode 100644 depth_image_proc/doc/changes.rst create mode 100644 depth_image_proc/doc/components.rst create mode 100644 depth_image_proc/doc/conf.py create mode 100644 depth_image_proc/doc/index.rst create mode 100644 depth_image_proc/doc/tutorials.rst diff --git a/depth_image_proc/doc/changes.rst b/depth_image_proc/doc/changes.rst new file mode 100644 index 000000000..852ee4122 --- /dev/null +++ b/depth_image_proc/doc/changes.rst @@ -0,0 +1,18 @@ +Changelog Notes +=============== + +Jazzy Jalisco +------------- +There are several major change between ``Iron`` and ``Jazzy``: + + * All components now properly support ``image_transport`` parameter, or + ``depth_image_transport`` if the topic is a depth image. + * All components now properly support remapping the ``camera_info`` topic + for an associated ``image`` topic. For instance, if you remap ``image`` + to ``my/image`` then ``my/camera_info`` will be used. Previously you + would have to manually remap the ``camera_info`` topic. + * The input of ``point_cloud_xyz_radial`` is renamed from ``image_raw`` + to ``depth/image_raw`` for consistency. + * The input of ``point_cloud_xyzrgb_radial`` is renamed from + ``depth_registered/image_rect`` to ``depth/image_raw`` and + ``rgb/image_rect_color`` to ``rgb/image_raw``. diff --git a/depth_image_proc/doc/components.rst b/depth_image_proc/doc/components.rst new file mode 100644 index 000000000..9f66818e5 --- /dev/null +++ b/depth_image_proc/doc/components.rst @@ -0,0 +1,178 @@ +Nodes and Components +==================== + +This package includes a number of ROS 2 components that can be assembled +into depth image processing pipelines. +See the tutorial :ref:`Launch depth_image_proc Components`. + +Alternatively, each component can be run as a standalone node. + +depth_image_proc::ConvertMetricNode +----------------------------------- +Component to convert raw uint16 depth image in mm to float depth image in m. +Also available as a standalone node with the name ``convert_metric_node``. + +Subscribed Topics +^^^^^^^^^^^^^^^^^ + * **image_raw** (sensor_msgs/Image): ``uint16`` depth image in mm, the native + OpenNI format. + +Published Topics +^^^^^^^^^^^^^^^^ + * **image** (sensor_msgs/Image): ``float`` depth image in m, the recommended + format for processing in ROS. + +Parameters +^^^^^^^^^^ + * **image_transport** (string, default: raw): Image transport to use. + +depth_image_proc::CropForemostNode +---------------------------------- +TODO + +depth_image_proc::DisparityNode +------------------------------- +Comoonent to convert depth image to disparity image. + +TODO: copy images in here + +Subscribed Topics +^^^^^^^^^^^^^^^^^ + * **left/image_rect** (sensor_msgs/Image): Rectified depth image. + * **right/camera_info** (sensor_msgs/CameraInfo): Camera calibration and + metadata. Must contain the baseline, which conventionally is encoded in + the right camera P matrix. + +Published Topics +^^^^^^^^^^^^^^^^ + * **left/disparity** (stereo_msgs/DisparityImage): Disparity image + (inversely related to depth), for interop with stereo processing nodes. + For all other purposes use depth images instead. + +Parameters +^^^^^^^^^^ + * **delta_d** (double, default: 0.125): Smallest allowed disparity increment, + which relates to the achievable depth range resolution. Defaults to 1/8 pixel. + * **image_transport** (string, default: raw): Image transport to use. + * **min_range** (double, default: 0.0): Minimum detectable distance. + * **max_range** (double, default: +Inf): Maximum detectable distance. + * **queue_size** (int, default: 5): Size of message queue for synchronizing + subscribed topics. + +depth_image_proc::PointCloudXyzNode +----------------------------------- +Component to convert depth image to XYZ point cloud. + +TODO: copy images + +Subscribed Topics +^^^^^^^^^^^^^^^^^ + * **depth/image_rect** (sensor_msgs/Image): Rectified depth image. + * **intensity/image_rect** (sensor_msgs/Image): Rectified intensity image. + * **camera_info** (sensor_msgs/CameraInfo): Camera calibration and metadata. + +Published Topics +^^^^^^^^^^^^^^^^ + * **points** (sensor_msgs/PointCloud2): XYZ point cloud. If using PCL, + subscribe as PointCloud. + +Parameters +^^^^^^^^^^ + * **depth_image_transport** (string, default: raw): Image transport to use + for the depth topic subscriber. + * **image_transport** (string, default: raw): Image transport to use for + the intensity image subscriber. + * **queue_size** (int, default: 5): Size of message queue for synchronizing + subscribed topics. + +depth_image_proc::PointCloudXyzRadialNode +----------------------------------------- +TODO + +depth_image_proc::PointCloudXyziNode +------------------------------------ +Component to convert depth image to XYZI point cloud. + +Subscribed Topics +^^^^^^^^^^^^^^^^^ + * **image_rect** (sensor_msgs/Image): Rectified depth image. + * **camera_info** (sensor_msgs/CameraInfo): Camera calibration and metadata. + +Published Topics +^^^^^^^^^^^^^^^^ + * **points** (sensor_msgs/PointCloud2): XYZ point cloud. If using PCL, + subscribe as PointCloud. + +Parameters +^^^^^^^^^^ + * **depth_image_transport** (string, default: raw): Image transport to use. + * **queue_size** (int, default: 5): Size of message queue for synchronizing + subscribed topics. + +depth_image_proc::PointCloudXyziRadialNode +------------------------------------------ +TODO + +depth_image_proc::PointCloudXyzrgbNode +-------------------------------------- +Component combine registered depth image and RGB image into XYZRGB point cloud. + +TODO: copy images + +Subscribed Topics +^^^^^^^^^^^^^^^^^ + * **depth_registered/image_rect** (sensor_msgs/Image): Rectified depth image, + registered to the RGB camera + * **rgb/image_rect_color** (sensor_msgs/Image): Rectified color image. + * **rgb/camera_info** (sensor_msgs/CameraInfo): RGB camera calibration and metadata. + +Published Topics +^^^^^^^^^^^^^^^^ + * **points** (sensor_msgs/PointCloud2): XYZ point cloud. If using PCL, + subscribe as PointCloud. + +Parameters +^^^^^^^^^^ + * **depth_image_transport** (string, default: raw): Image transport to use + for depth_registered subscriber. + * **image_transport** (string, default: raw): Image transport to use for + rgb/image_rect_color subscriber. + * **exact_sync** (bool, default: False): Whether to use exact synchronizer. + * **queue_size** (int, default: 5): Size of message queue for synchronizing + subscribed topics. + +depth_image_proc::PointCloudXyzrgbRadialNode +-------------------------------------------- +TODO + +depth_image_proc::RegisterNode +------------------------------ +Component to "register" a depth image to another camera frame. Reprojecting the +depths requires the calibration parameters of both cameras and, from tf, and the +extrinsic transform between them. + +Subscribed Topics +^^^^^^^^^^^^^^^^^ + * **depth/image_rect** (sensor_msgs/Image): Rectified depth image. + * **depth/camera_info** (sensor_msgs/CameraInfo): Depth camera calibration and metadata. + * **rgb/camera_info** (sensor_msgs/CameraInfo): RGB camera calibration and metadata. + +Published Topics +^^^^^^^^^^^^^^^^ + * **depth_registered/camera_info** (sensor_msgs/CameraInfo): Camera calibration and + metadata. Same as rgb/camera_info but time-synced to depth_registered/image_rect. + * **depth_registered/image_rect** (sensor_msgs/Image): Reprojected depth image in the + RGB camera frame. + +Parameters +^^^^^^^^^^ + * **depth_image_transport** (string, default: raw): Image transport to use + for depth subscriber. + * **queue_size** (int, default: 5): Size of message queue for synchronizing + subscribed topics. + +Required TF Transforms +^^^^^^^^^^^^^^^^^^^^^^ + * /depth_optical_frame → /rgb_optical_frame: The transform between the depth and + RGB camera optical frames as specified in the headers of the subscribed topics + (rendered here as /depth_optical_frame and /rgb_optical_frame). diff --git a/depth_image_proc/doc/conf.py b/depth_image_proc/doc/conf.py new file mode 100644 index 000000000..a69b4b083 --- /dev/null +++ b/depth_image_proc/doc/conf.py @@ -0,0 +1,194 @@ +# Copyright 2019 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. + +# -*- coding: utf-8 -*- +# +# Configuration file for the Sphinx documentation builder. +# +# This file does only contain a selection of the most common options. For a +# full list see the documentation: +# http://www.sphinx-doc.org/en/master/config + +# -- Path setup -------------------------------------------------------------- + +# 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. +# +import os +import sys +sys.path.insert(0, os.path.abspath('.')) + + +# -- Project information ----------------------------------------------------- + +project = 'depth_image_proc' +copyright = '2008-2024, Open Source Robotics Foundation, Inc.' # noqa +author = 'Open Source Robotics Foundation, Inc.' + +# The short X.Y version +version = '' +# The full version, including alpha/beta/rc tags +release = '3.2.1' + + +# -- 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.autosummary', + 'sphinx.ext.doctest', + 'sphinx.ext.coverage', + 'sphinx_rtd_theme', +] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# The suffix(es) of source filenames. +# You can specify multiple suffix as a list of string: +# +# source_suffix = ['.rst', '.md'] +source_suffix = '.rst' + +# The master toctree document. +master_doc = 'index' + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +# +# This is also used if you do content translation via gettext catalogs. +# Usually you set "language" from the command line for these cases. +language = None + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This pattern also affects html_static_path and html_extra_path. +exclude_patterns = [] + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = None + + +# -- 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 = 'sphinx_rtd_theme' + +# 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 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 = [] + +# Custom sidebar templates, must be a dictionary that maps document names +# to template names. +# +# The default sidebars (for documents that don't match any pattern) are +# defined by theme itself. Builtin themes are using these templates by +# default: ``['localtoc.html', 'relations.html', 'sourcelink.html', +# 'searchbox.html']``. +# +# html_sidebars = {} + + +# -- Options for HTMLHelp output --------------------------------------------- + +# Output file base name for HTML help builder. +htmlhelp_basename = 'depth_image_proc_doc' + + +# -- 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': '', + + # Latex figure (float) alignment + # + # 'figure_align': 'htbp', +} + + +# -- Options for manual page output ------------------------------------------ + +# One entry per manual page. List of tuples +# (source start file, name, description, authors, manual section). +man_pages = [ + (master_doc, 'depth_image_proc', 'depth_image_proc Documentation', + [author], 1) +] + + +# -- 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 = [ + (master_doc, 'depth_image_proc', 'depth_image_proc Documentation', + author, 'depth_image_proc', 'ROS 2 components for depth image processing.', + 'Miscellaneous'), +] + + +# -- Options for Epub output ------------------------------------------------- + +# Bibliographic Dublin Core info. +epub_title = project + +# The unique identifier of the text. This can be a ISBN number +# or the project homepage. +# +# epub_identifier = '' + +# A unique identification for the text. +# +# epub_uid = '' + +# A list of files that should not be packed into the epub file. +epub_exclude_files = ['search.html'] + + +# -- Extension configuration ------------------------------------------------- + +autoclass_content = 'both' + +autodoc_default_options = { + 'members': True, # document members + 'undoc-members': True, # also document members without documentation +} diff --git a/depth_image_proc/doc/index.rst b/depth_image_proc/doc/index.rst new file mode 100644 index 000000000..dd83269cc --- /dev/null +++ b/depth_image_proc/doc/index.rst @@ -0,0 +1,40 @@ +depth_image_proc +================ + +``depth_image_proc`` provides basic processing for depth images, much as +``image_proc`` does for traditional 2D images. The two packages are +complementary; for example, you can (and should!) rectify your depth +image before converting it to a point cloud. + +A variety of camera technologies can produce depth images: + + * The Kinect and related devices + * Traditional stereo cameras + * Time-of-flight cameras + +See REP 118 for details on depth image representation. The REP recommends +that, wherever possible, producers and consumers of depth data use depth +images (of type sensor_msgs/Image) instead of sensor_msgs/DisparityImage. + +All ROS 2 components (besides ``ConvertMetricNode``) in this package support +both standard floating point depth images and OpenNI-specific uint16 depth +images. Thus when working with OpenNI cameras (e.g. the Kinect), you can +save a few CPU cycles by using the uint16 raw topics instead of the float +topics. + +For an example of ``depth_image_proc`` in practice, examine the contents of +``openni2_launch``. + +.. toctree:: + :maxdepth: 2 + + components + tutorials + changes + image_proc + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`search` diff --git a/depth_image_proc/doc/tutorials.rst b/depth_image_proc/doc/tutorials.rst new file mode 100644 index 000000000..282f9f0ff --- /dev/null +++ b/depth_image_proc/doc/tutorials.rst @@ -0,0 +1,93 @@ +Tutorials +========= + +.. _Launch depth_image_proc Components: + +Launching depth_image_proc Components +------------------------------------- +While each of the components is available as a ROS 2 node, the +recommended way to build pipelines is using the components as +this will save overhead by not having to serialize messages +between components. + +The example below creates two composable nodes: + * ``rectify_node`` subscribes to ``image_raw``, rectifies the + image and publishes it at ``image_rect``. + * ``crop_decimage_node`` subscribes to the rectified + ``image_rect``, decimates it to half the size and publishes + the ``image_downsized``. + +.. code-block:: python + + from launch import LaunchDescription + from launch_ros.actions import ComposableNodeContainer + from launch_ros.descriptions import ComposableNode + + def generate_launch_description(): + + composable_nodes = [ + ComposableNode( + package='image_proc', + plugin='image_proc::RectifyNode', + name='rectify_node', + remappings=[ + ('image', 'image_raw'), + ('image_rect', 'image_rect') + ], + ), + ComposableNode( + package='image_proc', + plugin='image_proc::CropDecimateNode', + name='crop_decimate_node', + remappings=[ + ('in/image_raw', 'image_rect'), + ('out/image_raw', 'image_downsized') + ], + parameters={ + 'decimation_x': 2, + 'decimation_y': 2, + } + ) + ] + + container = ComposableNodeContainer( + name='image_proc_container', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=composable_nodes, + ) + + return LaunchDescription([container]) + +Using image_proc Launch File +---------------------------- +Make sure your camera driver is running. To see the available raw +image topics from compatible drivers you can check: + +``` +ros2 topic list | grep image_raw +``` + +Normally the raw image from the camera driver is not what you want +for visual processing, but rather an undistorted and (if necessary) +debayered image. This is the job of ``image_proc``. If you are +running on a robot, it's probably best to run ``image_proc`` there. +For example, if the driver is publishing topics ``/my_camera/image_raw`` +and ``/my_camera/camera_info`` you would do: + +``` +$ ros2 launch image_proc image_proc.launch.py namespace:=my_camera +``` + +Notice that we push our ``image_proc`` launch file down into the +``/my_camera`` namespace, in which it subscribes to the ``image_raw`` +and ``camera_info`` topics. All output topics are likewise published +within the ``/my_camera`` namespace. + +In a separate terminal (on your home machine, if you are running on a robot): + +``` +$ ros2 run image_view image_view --ros-args -r image:=my_camera/image_rect_color +``` + +This will display an undistorted color image from ``my_camera``. diff --git a/depth_image_proc/src/register.cpp b/depth_image_proc/src/register.cpp index 8a44136ec..b6cde396f 100644 --- a/depth_image_proc/src/register.cpp +++ b/depth_image_proc/src/register.cpp @@ -100,7 +100,7 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options) : rclcpp::Node("RegisterNode", options) { // TransportHints does not actually declare the parameter - this->declare_parameter("image_transport", "raw"); + this->declare_parameter("depth_image_transport", "raw"); rclcpp::Clock::SharedPtr clock = this->get_clock(); tf_buffer_ = std::make_shared(clock); @@ -137,7 +137,7 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options) // fully expanded and remapped topic name to image_transport auto node_base = this->get_node_base_interface(); std::string topic = node_base->resolve_topic_or_service_name("depth/image_rect", false); - image_transport::TransportHints hints(this); + image_transport::TransportHints hints(this, "raw", "depth_image_transport"); sub_depth_image_.subscribe(this, topic, hints.getTransport()); sub_depth_info_.subscribe(this, "depth/camera_info"); sub_rgb_info_.subscribe(this, "rgb/camera_info");