From 0eb00d064920dc5097afe21bf6846b23357cfc28 Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Tue, 29 Oct 2024 22:20:31 +0100 Subject: [PATCH] Initial commit Signed-off-by: Jose Miguel --- .github/workflows/master.yaml | 34 +++++ CMakeLists.txt | 78 ++++++++++ README.md | 81 +++++++++- launch/rgbd_stereo.launch.py | 214 ++++++++++++++++++++++++++ package.xml | 22 +++ rviz/rgbd_stereo_pcl.rviz | 252 +++++++++++++++++++++++++++++++ src/rgbd_stereo_publisher.cpp | 274 ++++++++++++++++++++++++++++++++++ 7 files changed, 954 insertions(+), 1 deletion(-) create mode 100644 .github/workflows/master.yaml create mode 100644 CMakeLists.txt create mode 100644 launch/rgbd_stereo.launch.py create mode 100644 package.xml create mode 100644 rviz/rgbd_stereo_pcl.rviz create mode 100644 src/rgbd_stereo_publisher.cpp diff --git a/.github/workflows/master.yaml b/.github/workflows/master.yaml new file mode 100644 index 0000000..9587778 --- /dev/null +++ b/.github/workflows/master.yaml @@ -0,0 +1,34 @@ +name: jazzy + +on: + pull_request: + branches: + - jazzy + push: + branches: + - jazzy + +jobs: + build-and-test: + runs-on: ${{ matrix.os }} + container: + image: osrf/ros:jazzy-desktop + strategy: + matrix: + os: [ubuntu-24.04] + fail-fast: false + steps: + - name: Install deps + run: sudo apt-get update && sudo apt-get install -y wget python3-vcstool python3-colcon-coveragepy-result + - name: build and test + uses: ros-tooling/action-ros-ci@0.3.15 + with: + package-name: oak_d_lite_camera_ros2 + target-ros2-distro: jazzy + vcs-repo-file-url: "" + colcon-defaults: | + { + "test": { + "parallel-workers" : 1 + } + } diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..3a1474a --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,78 @@ +cmake_minimum_required(VERSION 3.10.2) +project(oak_d_lite_camera_ros2 VERSION 0.0.1 LANGUAGES CXX C) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) +add_compile_options(-g) + +## is used, also find other catkin packages +if(POLICY CMP0057) + cmake_policy(SET CMP0057 NEW) +endif() + +set(_opencv_version 4) +find_package(OpenCV 4 QUIET COMPONENTS imgproc highgui) +if(NOT OpenCV_FOUND) + set(_opencv_version 3) + find_package(OpenCV 3 REQUIRED COMPONENTS imgproc highgui) +endif() + +find_package(ament_cmake REQUIRED) + +message(STATUS "------------------------------------------") +message(STATUS "Depthai Bridge is being built using AMENT.") +message(STATUS "------------------------------------------") + +find_package(camera_info_manager REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(depthai CONFIG REQUIRED) +find_package(depthai_ros_msgs REQUIRED) +find_package(depthai_bridge REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(stereo_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(vision_msgs REQUIRED) + +set(dependencies + camera_info_manager + cv_bridge + depthai_ros_msgs + depthai_bridge + rclcpp + sensor_msgs + stereo_msgs + std_msgs + vision_msgs +) + +include_directories( + include + ${ament_INCLUDE_DIRS} +) + +macro(dai_add_node_ros2 node_name node_src) + add_executable("${node_name}" "${node_src}") + + target_link_libraries("${node_name}" + depthai::core + opencv_imgproc + opencv_highgui) + + ament_target_dependencies("${node_name}" + ${dependencies}) + +endmacro() + +dai_add_node_ros2(rgbd_stereo_node src/rgbd_stereo_publisher.cpp) + +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + +install(TARGETS + rgbd_stereo_node + DESTINATION lib/${PROJECT_NAME}) + +ament_package() + diff --git a/README.md b/README.md index 43c9d93..823e05c 100644 --- a/README.md +++ b/README.md @@ -1 +1,80 @@ -# oak_d_lite_camera_ros2 \ No newline at end of file +# OAK-D-LITE Camera ROS 2 Launcher + +![distro](https://img.shields.io/badge/Ubuntu%2024-Nobley%20Numbat-green) +![distro](https://img.shields.io/badge/ROS2-Jazzy-blue) +[![jazzy](https://github.com/jmguerreroh/oak_d_lite_camera_ros2/actions/workflows/master.yaml/badge.svg?branch=jazzy)](https://github.com/jmguerreroh/oak_d_lite_camera_ros2/actions/workflows/master.yaml) + +This package provides a ROS 2 launch file to initialize and configure the OAK-D-LITE camera for RGBD, stereo, and point cloud processing. It includes adjustable camera configurations for transform frames, position, orientation, and depth processing parameters, along with a sample RViz configuration. + +## Installation + +1. Clone the repository and add it to your ROS 2 workspace: + ```bash + git clone https://github.com/jmguerreroh/oak_d_lite_camera_ros2.git + ``` + +2. Install dependencies: + ```bash + rosdep install --from-paths src --ignore-src -r -y + ``` + +3. Build the workspace: + ```bash + colcon build --symlink-install + ``` + +## Usage + +To launch the OAK-D-LITE camera node with default parameters: + +```bash +ros2 launch oak_d_lite_camera_ros2_launcher oak_d_lite_camera.launch.py +``` + +## Launch Arguments + +The launch file provides several configurable arguments: + +#### Camera Parameters + +- `camera_model`: Model of the camera. Options: `OAK-D`, `OAK-D-LITE`. Default: `OAK-D-LITE` +- `tf_prefix`: TF prefix for camera frames. Default: `oak` +- `base_frame`: Name of the camera's base frame. Default: `oak-d_frame` +- `parent_frame`: Parent frame for the camera's base. Default: `oak-d-base-frame` + +#### Camera Position and Orientation + +- `cam_pos_x`, `cam_pos_y`, `cam_pos_z`: Position of the camera relative to the base frame. Default: `0.0` +- `cam_roll`, `cam_pitch`, `cam_yaw`: Orientation of the camera relative to the base frame. Default: `0.0` + +#### Depth Processing Parameters + +- `mode`: Depth processing mode (`depth` or `disparity`). Default: `depth` +- `lrcheck`: Left-right consistency check. Default: `True` +- `extended`: Enable extended disparity range. Default: `False` +- `subpixel`: Enable subpixel accuracy. Default: `True` +- `confidence`: Confidence threshold for depth estimation. Default: `200` +- `LRchecktresh`: Left-right consistency threshold. Default: `5` + +## Visualizing in RViz + +The launch file opens RViz with a pre-configured view. You can modify `rviz/rgbd_stereo_pcl.rviz` to customize the visualization. + + +## About + +This project was made by [José Miguel Guerrero], Associate Professor at [Universidad Rey Juan Carlos]. + +Copyright © 2024. + +[![Twitter](https://img.shields.io/badge/follow-@jm__guerrero-green.svg)](https://twitter.com/jm__guerrero) + +## License + +This work is licensed under the terms of the [MIT license](https://opensource.org/license/mit). + +[![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) + +[Universidad Rey Juan Carlos]: https://www.urjc.es/ +[José Miguel Guerrero]: https://sites.google.com/view/jmguerrero + diff --git a/launch/rgbd_stereo.launch.py b/launch/rgbd_stereo.launch.py new file mode 100644 index 0000000..4aa8d59 --- /dev/null +++ b/launch/rgbd_stereo.launch.py @@ -0,0 +1,214 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription, launch_description_sources +from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +import launch_ros.actions +import launch_ros.descriptions + + +def generate_launch_description(): + default_rviz = os.path.join(get_package_share_directory('oak_d_lite_camera_ros2'), + 'rviz', 'rgbd_stereo_pcl.rviz') + urdf_launch_dir = os.path.join(get_package_share_directory('depthai_descriptions'), 'launch') + + + camera_model = LaunchConfiguration('camera_model', default = 'OAK-D-LITE') + tf_prefix = LaunchConfiguration('tf_prefix', default = 'oak') + base_frame = LaunchConfiguration('base_frame', default = 'oak-d_frame') + parent_frame = LaunchConfiguration('parent_frame', default = 'oak-d-base-frame') + + cam_pos_x = LaunchConfiguration('cam_pos_x', default = '0.0') + cam_pos_y = LaunchConfiguration('cam_pos_y', default = '0.0') + cam_pos_z = LaunchConfiguration('cam_pos_z', default = '0.0') + cam_roll = LaunchConfiguration('cam_roll', default = '0.0') + cam_pitch = LaunchConfiguration('cam_pitch', default = '0.0') + cam_yaw = LaunchConfiguration('cam_yaw', default = '0.0') + + mode = LaunchConfiguration('mode', default = 'depth') + lrcheck = LaunchConfiguration('lrcheck', default = True) + extended = LaunchConfiguration('extended', default = False) + subpixel = LaunchConfiguration('subpixel', default = True) + confidence = LaunchConfiguration('confidence', default = 200) + LRchecktresh = LaunchConfiguration('LRchecktresh', default = 5) + + + declare_camera_model_cmd = DeclareLaunchArgument( + 'camera_model', + default_value=camera_model, + description='The model of the camera. Using a wrong camera model can disable camera features. Valid models: `OAK-D, OAK-D-LITE`.') + + declare_tf_prefix_cmd = DeclareLaunchArgument( + 'tf_prefix', + default_value=tf_prefix, + description='The name of the camera. It can be different from the camera model and it will be used in naming TF.') + + declare_base_frame_cmd = DeclareLaunchArgument( + 'base_frame', + default_value=base_frame, + description='Name of the base link.') + + declare_parent_frame_cmd = DeclareLaunchArgument( + 'parent_frame', + default_value=parent_frame, + description='Name of the parent link from other a robot TF for example that can be connected to the base of the OAK.') + + declare_pos_x_cmd = DeclareLaunchArgument( + 'cam_pos_x', + default_value=cam_pos_x, + description='Position X of the camera with respect to the base frame.') + + declare_pos_y_cmd = DeclareLaunchArgument( + 'cam_pos_y', + default_value=cam_pos_y, + description='Position Y of the camera with respect to the base frame.') + + declare_pos_z_cmd = DeclareLaunchArgument( + 'cam_pos_z', + default_value=cam_pos_z, + description='Position Z of the camera with respect to the base frame.') + + declare_roll_cmd = DeclareLaunchArgument( + 'cam_roll', + default_value=cam_roll, + description='Roll orientation of the camera with respect to the base frame.') + + declare_pitch_cmd = DeclareLaunchArgument( + 'cam_pitch', + default_value=cam_pitch, + description='Pitch orientation of the camera with respect to the base frame.') + + declare_yaw_cmd = DeclareLaunchArgument( + 'cam_yaw', + default_value=cam_yaw, + description='Yaw orientation of the camera with respect to the base frame.') + + declare_mode_cmd = DeclareLaunchArgument( + 'mode', + default_value=mode, + description='set to depth or disparity. Setting to depth will publish depth or else will publish disparity.') + + declare_lrcheck_cmd = DeclareLaunchArgument( + 'lrcheck', + default_value=lrcheck, + description='The name of the camera. It can be different from the camera model and it will be used as node `namespace`.') + + declare_extended_cmd = DeclareLaunchArgument( + 'extended', + default_value=extended, + description='The name of the camera. It can be different from the camera model and it will be used as node `namespace`.') + + declare_subpixel_cmd = DeclareLaunchArgument( + 'subpixel', + default_value=subpixel, + description='The name of the camera. It can be different from the camera model and it will be used as node `namespace`.') + + declare_confidence_cmd = DeclareLaunchArgument( + 'confidence', + default_value=confidence, + description='The name of the camera. It can be different from the camera model and it will be used as node `namespace`.') + + declare_LRchecktresh_cmd = DeclareLaunchArgument( + 'LRchecktresh', + default_value=LRchecktresh, + description='The name of the camera. It can be different from the camera model and it will be used as node `namespace`.') + + + urdf_launch = IncludeLaunchDescription( + launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(urdf_launch_dir, 'urdf_launch.py')), + launch_arguments={'tf_prefix' : tf_prefix, + 'camera_model': camera_model, + 'base_frame' : base_frame, + 'parent_frame': parent_frame, + 'cam_pos_x' : cam_pos_x, + 'cam_pos_y' : cam_pos_y, + 'cam_pos_z' : cam_pos_z, + 'cam_roll' : cam_roll, + 'cam_pitch' : cam_pitch, + 'cam_yaw' : cam_yaw}.items()) + + rgbd_stereo_node = launch_ros.actions.Node( + package='oak_d_lite_camera_ros2', executable='rgbd_stereo_node', + output='screen', + parameters=[{'tf_prefix': tf_prefix}, + {'mode': mode}, + {'lrcheck': lrcheck}, + {'extended': extended}, + {'subpixel': subpixel}, + {'confidence': confidence}, + {'LRchecktresh': LRchecktresh}]) + + metric_converter_node = launch_ros.actions.ComposableNodeContainer( + name='metric_converter_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + # Driver itself + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::ConvertMetricNode', + name='convert_metric_node', + remappings=[('image_raw', '/stereo/depth'), + ('camera_info', '/stereo/camera_info'), + ('image', '/stereo/converted_depth')] + ), + ], + output='screen',) + + point_cloud_color_node = launch_ros.actions.ComposableNodeContainer( + name='pcl_color_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + # Driver itself + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::PointCloudXyzrgbNode', + name='point_cloud_xyzrgb_node', + remappings=[('depth_registered/image_rect', 'stereo/converted_depth'), + ('rgb/image_rect_color', '/rgb/image'), + ('rgb/camera_info', '/rgb/camera_info'), + ('points', '/stereo/points_color' )] + ), + ], + output='screen',) + + rviz_node = launch_ros.actions.Node( + package='rviz2', executable='rviz2', output='screen', + arguments=['--display-config', default_rviz]) + + + ld = LaunchDescription() + ld.add_action(declare_tf_prefix_cmd) + ld.add_action(declare_camera_model_cmd) + + ld.add_action(declare_base_frame_cmd) + ld.add_action(declare_parent_frame_cmd) + + ld.add_action(declare_pos_x_cmd) + ld.add_action(declare_pos_y_cmd) + ld.add_action(declare_pos_z_cmd) + ld.add_action(declare_roll_cmd) + ld.add_action(declare_pitch_cmd) + ld.add_action(declare_yaw_cmd) + + ld.add_action(declare_mode_cmd) + ld.add_action(declare_lrcheck_cmd) + ld.add_action(declare_extended_cmd) + ld.add_action(declare_subpixel_cmd) + ld.add_action(declare_confidence_cmd) + ld.add_action(declare_LRchecktresh_cmd) + + ld.add_action(rgbd_stereo_node) + ld.add_action(urdf_launch) + + ld.add_action(metric_converter_node) + ld.add_action(point_cloud_color_node) + ld.add_action(rviz_node) + return ld + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..d3e20da --- /dev/null +++ b/package.xml @@ -0,0 +1,22 @@ + + + + oak_d_lite_camera_ros2 + 1.0.0 + OAK-D-LITE camera package + Jose Miguel Guerrero Hernandez + MIT License + ament_cmake + + rclcpp + depthai-ros + + robot_state_publisher + depth_image_proc + xacro + + + ament_cmake + + + diff --git a/rviz/rgbd_stereo_pcl.rviz b/rviz/rgbd_stereo_pcl.rviz new file mode 100644 index 0000000..a4bf56d --- /dev/null +++ b/rviz/rgbd_stereo_pcl.rviz @@ -0,0 +1,252 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21/Topic1 + Splitter Ratio: 0.5 + Tree Height: 254 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 28.555692672729492 + Min Value: -27.46285057067871 + Value: true + Axis: Z + Channel Name: z + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 45.77400207519531 + Min Color: 0; 0; 0 + Min Intensity: 0.36100003123283386 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: System Default + Reliability Policy: Best Effort + Value: /stereo/points_color + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + oak-d-base-frame: + Alpha: 1 + Show Axes: false + Show Trail: false + oak-d_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + oak_left_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + oak_left_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + oak_model_origin: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + oak_rgb_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + oak_rgb_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + oak_right_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + oak_right_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: left_rect + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /left_rect/image + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: right_rect + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /right_rect/image + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: oak-d_frame + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 5.273627281188965 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.7700800895690918 + Y: -0.4642658233642578 + Z: 1.758968710899353 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6297970414161682 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 3.1803972721099854 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 794 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000274000002bcfc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f0000013e000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000160063006f006c006f0072005f0069006d0061006700650000000256000000a20000000000000000fb0000001400720069006700680074005f00720065006300740100000183000000b50000001700fffffffb0000002c0072006500670069007300740065007200650064005f00640065007000740068005f0069006d006100670065000000038b000000d10000000000000000fb00000012006c006500660074005f0072006500630074010000023e000000bd0000001700ffffff000000010000010f000002bcfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002bc000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002d3000002bc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1634 + X: 169 + Y: 52 + left_rect: + collapsed: false + right_rect: + collapsed: false diff --git a/src/rgbd_stereo_publisher.cpp b/src/rgbd_stereo_publisher.cpp new file mode 100644 index 0000000..af98dfb --- /dev/null +++ b/src/rgbd_stereo_publisher.cpp @@ -0,0 +1,274 @@ +#include +#include +#include +#include + +#include "camera_info_manager/camera_info_manager.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/node.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "stereo_msgs/msg/disparity_image.hpp" + +// Includes DepthAI libraries needed to work with the OAK-D device and pipeline +#include "depthai/device/DataQueue.hpp" +#include "depthai/device/Device.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/node/MonoCamera.hpp" +#include "depthai/pipeline/node/StereoDepth.hpp" +#include "depthai/pipeline/node/XLinkOut.hpp" +#include "depthai_bridge/BridgePublisher.hpp" +#include "depthai_bridge/DisparityConverter.hpp" +#include "depthai_bridge/ImageConverter.hpp" +#include "depthai/pipeline/node/ColorCamera.hpp" + +// Function to configure and create the DepthAI pipeline +std::tuple createPipeline( + bool withDepth, bool lrcheck, bool extended, bool subpixel, int confidence, int LRchecktresh) +{ + + dai::Pipeline pipeline; // Creates the processing pipeline + pipeline.setXLinkChunkSize(0); // Disables chunk size for XLink transfer + dai::node::MonoCamera::Properties::SensorResolution monoResolution = + dai::node::MonoCamera::Properties::SensorResolution::THE_480_P; + dai::ColorCameraProperties::SensorResolution colorResolution = + dai::ColorCameraProperties::SensorResolution::THE_1080_P; + int stereoWidth = 640, stereoHeight = 480, rgbWidth = 1920, rgbHeight = 1080; + +// Creates nodes for mono cameras, RGB, and XLink output connections + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto xoutLeft = pipeline.create(); + auto xoutRight = pipeline.create(); + auto xoutLeftRect = pipeline.create(); + auto xoutRightRect = pipeline.create(); + auto stereo = pipeline.create(); + auto xoutDepth = pipeline.create(); + auto rgbCam = pipeline.create(); + auto xoutRgb = pipeline.create(); + +// Configures stream names for outputs + xoutLeft->setStreamName("left"); + xoutRight->setStreamName("right"); + xoutRgb->setStreamName("rgb"); + xoutLeftRect->setStreamName("left_rect"); + xoutRightRect->setStreamName("right_rect"); + + if(withDepth) { + xoutDepth->setStreamName("depth"); // Sets depth output stream + } else { + xoutDepth->setStreamName("disparity"); // Sets disparity output stream + } + + // Mono camera configuration (left and right) + monoLeft->setResolution(monoResolution); + monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B); // Sets camera socket + monoLeft->setFps(40); // Sets FPS + monoRight->setResolution(monoResolution); + monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C); + monoRight->setFps(40); + + // Stereo depth node configuration + stereo->initialConfig.setConfidenceThreshold(confidence); // Confidence threshold + stereo->setRectifyEdgeFillColor(0); // Black edge fill color for better cropping view + stereo->initialConfig.setLeftRightCheckThreshold(LRchecktresh); + stereo->setLeftRightCheck(lrcheck); // Enable/disable left-right check + stereo->setExtendedDisparity(extended); // Enable extended disparity + stereo->setSubpixel(subpixel); // Enable subpixel mode + if(withDepth) {stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);} + + // RGB camera configuration + rgbCam->setBoardSocket(dai::CameraBoardSocket::CAM_A); + rgbCam->setResolution(colorResolution); + rgbCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + rgbCam->setFps(35); + + // Link mono cameras to stereo depth node + monoLeft->out.link(stereo->left); + monoRight->out.link(stereo->right); + + // Link RGB camera to its XLink output + rgbCam->video.link(xoutRgb->input); + + // Link mono cameras to both rectified and unrectified outputs + stereo->syncedLeft.link(xoutLeft->input); + stereo->syncedRight.link(xoutRight->input); + stereo->rectifiedLeft.link(xoutLeftRect->input); + stereo->rectifiedRight.link(xoutRightRect->input); + + // Set depth or disparity output depending on mode + if(withDepth) { + stereo->depth.link(xoutDepth->input); + } else { + stereo->disparity.link(xoutDepth->input); + } + + // Returns the pipeline and configured dimensions + return std::make_tuple(pipeline, stereoWidth, stereoHeight, rgbWidth, rgbHeight); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); // Initializes ROS + auto node = rclcpp::Node::make_shared("rgbd_stereo_node"); // Creates a ROS node + + // Declare and get ROS parameters + std::string tfPrefix, mode; + bool lrcheck, extended, subpixel, enableDepth; + int confidence, LRchecktresh; + int monoWidth, monoHeight, colorWidth, colorHeight; + dai::Pipeline pipeline; + + node->declare_parameter("tf_prefix", "oak"); + node->declare_parameter("mode", "depth"); + node->declare_parameter("lrcheck", true); + node->declare_parameter("extended", false); + node->declare_parameter("subpixel", true); + node->declare_parameter("confidence", 200); + node->declare_parameter("LRchecktresh", 5); + + node->get_parameter("tf_prefix", tfPrefix); + node->get_parameter("mode", mode); + node->get_parameter("lrcheck", lrcheck); + node->get_parameter("extended", extended); + node->get_parameter("subpixel", subpixel); + node->get_parameter("confidence", confidence); + node->get_parameter("LRchecktresh", LRchecktresh); + + // Enable depth or disparity mode based on `mode` parameter + if(mode == "depth") { + enableDepth = true; + } else { + enableDepth = false; + } + + // Creates the pipeline with specified parameters + std::tie(pipeline, monoWidth, monoHeight, colorWidth, colorHeight) = createPipeline( + enableDepth, lrcheck, extended, subpixel, confidence, LRchecktresh); + + // Initialize DepthAI device with configured pipeline + dai::Device device(pipeline); + auto leftQueue = device.getOutputQueue("left", 30, false); + auto rightQueue = device.getOutputQueue("right", 30, false); + auto rgbQueue = device.getOutputQueue("rgb", 30, false); + auto leftRectQueue = device.getOutputQueue("left_rect", 30, false); + auto rightRectQueue = device.getOutputQueue("right_rect", 30, false); + + // Output queue for depth or disparity, depending on mode + std::shared_ptr stereoQueue; + if(enableDepth) { + stereoQueue = device.getOutputQueue("depth", 30, false); + } else { + stereoQueue = device.getOutputQueue("disparity", 30, false); + } + + // Converts and publishes image data to ROS + auto calibrationHandler = device.readCalibration(); + dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", false); + auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, + dai::CameraBoardSocket::CAM_A, colorWidth, colorHeight); + dai::rosBridge::BridgePublisher rgbPublish( + rgbQueue, + node, + std::string("rgb/image"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, + &rgbConverter, // since the converter has the same frame name + // and image type is also same we can reuse it + std::placeholders::_1, + std::placeholders::_2), + 30, + rgbCameraInfo, + "rgb"); + + rgbPublish.addPublisherCallback(); // addPublisherCallback works only when the dataqueue is non blocking. + + dai::rosBridge::ImageConverter leftconverter(tfPrefix + "_left_camera_optical_frame", true); + auto leftCameraInfo = leftconverter.calibrationToCameraInfo(calibrationHandler, + dai::CameraBoardSocket::CAM_B, monoWidth, monoHeight); + dai::rosBridge::BridgePublisher leftPublish( + leftQueue, + node, + std::string("left/image"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &leftconverter, std::placeholders::_1, + std::placeholders::_2), + 30, + leftCameraInfo, + "left"); + + leftPublish.addPublisherCallback(); + + dai::rosBridge::ImageConverter rightconverter(tfPrefix + "_right_camera_optical_frame", true); + auto rightCameraInfo = leftconverter.calibrationToCameraInfo(calibrationHandler, + dai::CameraBoardSocket::CAM_C, monoWidth, monoHeight); + + dai::rosBridge::BridgePublisher rightPublish( + rightQueue, + node, + std::string("right/image"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &rightconverter, std::placeholders::_1, + std::placeholders::_2), + 30, + rightCameraInfo, + "right"); + + rightPublish.addPublisherCallback(); + + dai::rosBridge::BridgePublisher leftRectPublish( + leftRectQueue, + node, + std::string("left_rect/image"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &leftconverter, std::placeholders::_1, + std::placeholders::_2), + 30, + leftCameraInfo, + "left_rect"); + + leftRectPublish.addPublisherCallback(); + + dai::rosBridge::BridgePublisher rightRectPublish( + rightRectQueue, + node, + std::string("right_rect/image"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &rightconverter, std::placeholders::_1, + std::placeholders::_2), + 30, + rightCameraInfo, + "right_rect"); + + rightRectPublish.addPublisherCallback(); + + // Converts and publishes depth or disparity images to ROS + if(mode == "depth") { + auto depthCameraInfo = enableDepth ? rgbCameraInfo : rightCameraInfo; + auto depthconverter = enableDepth ? rgbConverter : rightconverter; + dai::rosBridge::BridgePublisher depthPublish( + stereoQueue, + node, + std::string("stereo/depth"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, + &depthconverter, // since the converter has the same frame name + // and image type is also same we can reuse it + std::placeholders::_1, + std::placeholders::_2), + 30, + depthCameraInfo, + "stereo"); + depthPublish.addPublisherCallback(); + rclcpp::spin(node); + } else { + dai::rosBridge::DisparityConverter dispConverter(tfPrefix + "_right_camera_optical_frame", 880, + 7.5, 20, 2000); + dai::rosBridge::BridgePublisher dispPublish( + stereoQueue, + node, + std::string("stereo/disparity"), + std::bind(&dai::rosBridge::DisparityConverter::toRosMsg, &dispConverter, + std::placeholders::_1, std::placeholders::_2), + 30, + rightCameraInfo, + "stereo"); + dispPublish.addPublisherCallback(); + rclcpp::spin(node); + } + + return 0; +}