mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-10-03 19:47:46 +08:00
Merge branch 'v2-main' into test_interleave_mode_opdk
This commit is contained in:
datean
Vendored
+3
-1
@@ -72,6 +72,8 @@
|
||||
"typeindex": "cpp",
|
||||
"typeinfo": "cpp",
|
||||
"variant": "cpp",
|
||||
"*.ipp": "cpp"
|
||||
"*.ipp": "cpp",
|
||||
"filesystem": "cpp",
|
||||
"shared_mutex": "cpp"
|
||||
}
|
||||
}
|
||||
@@ -1,58 +1,177 @@
|
||||
# OrbbecSDK ROS2
|
||||
# OrbbecSDK ROS2 Wrapper v2
|
||||
|
||||
[](http://github.com/badges/stability-badges) 
|
||||
|
||||
OrbbecSDK_ROS2 is a wrapper for the Orbbec 3D camera that provides seamless integration with the ROS 2 environment. It
|
||||
supports ROS 2 Foxy, Humble, and Jazzy distributions.
|
||||
> [!IMPORTANT]
|
||||
>
|
||||
> Welcome to the OrbbecSDK ROS2 Wrapper v2. Before you begin using this version of ROS2 wrapper, it's crucial to check the following device support list to verify the compatibility.
|
||||
|
||||
With the major update of the new branch [v2-main](https://github.com/orbbec/OrbbecSDK_ROS2/tree/v2-main?tab=readme-ov-file) in October 2024, OrbbecSDK_ROS2 is connected to the open source version of [OrbbecSDK v2](https://github.com/orbbec/OrbbecSDK_v2/releases), which will make OrbbecSDK_ROS2 more flexible and extensible. This update in v2-main ensures compatibility with all new Orbbec USB products that comply with the UVC standard. However, OrbbecSDK_ROS2 v2 no longer supports Orbbec's traditional OpenNI protocol devices. We encourage you to check whether your device is supported by OrbbecSDK_ROS2 v2 and use the new version if supported.
|
||||
OrbbecSDK ROS2 Wrapper provides seamless integration of Orbbec cameras with ROS 2 environment. It supports ROS2 Foxy, Humble, and Jazzy distributions.
|
||||
|
||||
With a major update in October 2024, we release the [OrbbecSDK ROS2 Wrapper v2](https://github.com/orbbec/OrbbecSDK_ROS2/tree/v2-main) connected to the open source [OrbbecSDK v2](https://github.com/orbbec/OrbbecSDK_v2/releases) with enhanced flexibility and extensibility. This update ensures compatibility with all Orbbec USB products adhering to UVC standard. However, it no longer supports Orbbec's traditional OpenNI protocol devices. We strongly encourage you to use the v2-main branch if your device is supported.
|
||||
|
||||
Here is the device support list of main branch (v1.x) and v2-main branch (v2.x):
|
||||
|
||||
<table border="1" style="border-collapse: collapse; text-align: left; width: 100%;">
|
||||
<thead>
|
||||
<tr style="background-color: #1f4e78; color: white; text-align: center;">
|
||||
<th>Product Series</th>
|
||||
<th>Product</th>
|
||||
<th><a href="https://github.com/orbbec/OrbbecSDK_ROS2/tree/main" style="color: black; text-decoration: none;">Branch main</a></th>
|
||||
<th><a href="https://github.com/orbbec/OrbbecSDK_ROS2/tree/v2-main" style="color: black; text-decoration: none;">Branch v2-main</a></th>
|
||||
</tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr>
|
||||
<td rowspan="7" style="text-align: center; font-weight: bold;">Gemini 330</td>
|
||||
<td>Gemini 335</td>
|
||||
<td>full maintenance</td>
|
||||
<td>recommended for new designs</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Gemini 336</td>
|
||||
<td>full maintenance</td>
|
||||
<td>recommended for new designs</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Gemini 330</td>
|
||||
<td>full maintenance</td>
|
||||
<td>recommended for new designs</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Gemini 335L</td>
|
||||
<td>full maintenance</td>
|
||||
<td>recommended for new designs</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Gemini 336L</td>
|
||||
<td>full maintenance</td>
|
||||
<td>recommended for new designs</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Gemini 330L</td>
|
||||
<td>full maintenance</td>
|
||||
<td>recommended for new designs</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Gemini 335Lg</td>
|
||||
<td>not supported</td>
|
||||
<td>recommended for new designs</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td rowspan="3" style="text-align: center; font-weight: bold;">Gemini 2</td>
|
||||
<td>Gemini 2</td>
|
||||
<td>full maintenance</td>
|
||||
<td>recommended for new designs</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Gemini 2 L</td>
|
||||
<td>full maintenance</td>
|
||||
<td>recommended for new designs</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Gemini 2 XL</td>
|
||||
<td>recommended for new designs</td>
|
||||
<td>to be supported</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td rowspan="3" style="text-align: center; font-weight: bold;">Femto</td>
|
||||
<td>Femto Bolt</td>
|
||||
<td>full maintenance</td>
|
||||
<td>recommended for new designs</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Femto Mega</td>
|
||||
<td>full maintenance</td>
|
||||
<td>recommended for new designs</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Femto Mega I</td>
|
||||
<td>full maintenance</td>
|
||||
<td>to be supported</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td rowspan="3" style="text-align: center; font-weight: bold;">Astra</td>
|
||||
<td>Astra 2</td>
|
||||
<td>full maintenance</td>
|
||||
<td>recommended for new designs</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Astra+</td>
|
||||
<td>limited maintenance</td>
|
||||
<td>not supported</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Astra Pro Plus</td>
|
||||
<td>limited maintenance</td>
|
||||
<td>not supported</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td style="text-align: center; font-weight: bold;">Astra Mini</td>
|
||||
<td>Astra Mini Pro</td>
|
||||
<td>full maintenance</td>
|
||||
<td>not supported</td>
|
||||
</tr>
|
||||
</tbody>
|
||||
</table>
|
||||
|
||||
|
||||
|
||||
**Note**: If you do not find your device, please contact our FAE or sales representative for help.
|
||||
|
||||
**Definition**:
|
||||
|
||||
1. recommended for new designs: we will provide full supports with new features, bug fix and performance optimization;
|
||||
|
||||
2. full maintenance: we will provide bug fix support;
|
||||
|
||||
3. limited maintenance: we will provide critical bug fix support;
|
||||
|
||||
4. not supported: we will not support specific device in this version;
|
||||
|
||||
5. to be supported: we will add support in the near future.
|
||||
|
||||
## Table of Contents
|
||||
|
||||
<!-- TOC -->
|
||||
|
||||
* [OrbbecSDK ROS2](#orbbec-ros2-sdk)
|
||||
* [Table of Contents](#table-of-contents)
|
||||
* [Installation Instructions](#installation-instructions)
|
||||
* [Getting start](#getting-start)
|
||||
* [Efficient intra-process communication:](#efficient-intra-process-communication)
|
||||
* [Example](#example)
|
||||
* [Manually loading multiple components into the same process](#manually-loading-multiple-components-into-the-same-process)
|
||||
* [Using a launch file](#using-a-launch-file)
|
||||
* [Limitations](#limitations)
|
||||
* [Use V4L2 backend](#use-v4l2-backend)
|
||||
* [Launch parameters](#launch-parameters)
|
||||
* [ROS2-vs-Optical Coordination Systems](#ros2robot-vs-opticalcamera-coordination-systems)
|
||||
* [Camera sensor structure](#camera-sensor-structure)
|
||||
* [TF from coordinate A to coordinate B:](#tf-from-coordinate-a-to-coordinate-b)
|
||||
* [Predefined presets](#predefined-presets)
|
||||
* [Depth work mode switch](#depth-work-mode-switch)
|
||||
* [Configuration of depth NFOV and WFOV modes](#configuration-of-depth-nfov-and-wfov-modes)
|
||||
* [All available service for camera control](#all-available-service-for-camera-control)
|
||||
* [All available topics](#all-available-topics)
|
||||
* [Network device enumeration](#network-device-enumeration)
|
||||
* [Multi-Camera](#multi-camera)
|
||||
* [Compressed Image](#compressed-image)
|
||||
* [Use hardware decoder to decode JPEG](#use-hardware-decoder-to-decode-jpeg)
|
||||
* [rockchip and Amlogic](#rockchip-and-amlogic)
|
||||
* [Nvidia Jetson](#nvidia-jetson)
|
||||
* [Check which profiles the camera supports](#check-which-profiles-the-camera-supports)
|
||||
* [Building a Debian Package](#building-a-debian-package)
|
||||
* [Preparing the Environment](#preparing-the-environment)
|
||||
* [Configuring ROS Dependencies](#configuring-ros-dependencies)
|
||||
* [Building the Package](#building-the-package)
|
||||
* [Launch files](#launch-files)
|
||||
* [Product support](#product-support)
|
||||
* [DDS Tuning](#dds-tuning)
|
||||
* [Frequently Asked Questions](#frequently-asked-questions)
|
||||
* [Unexpected Crash](#unexpected-crash)
|
||||
* [No Data Stream from Multiple Cameras](#no-data-stream-from-multiple-cameras)
|
||||
* [Additional Troubleshooting](#additional-troubleshooting)
|
||||
* [Why Are There So Many Launch Files?](#why-are-there-so-many-launch-files)
|
||||
* [Other useful links](#other-useful-links)
|
||||
* [License](#license)
|
||||
|
||||
<!-- TOC -->
|
||||
- [OrbbecSDK ROS2 Wrapper v2](#orbbecsdk-ros2-wrapper-v2)
|
||||
- [Table of Contents](#table-of-contents)
|
||||
- [Installation Instructions](#installation-instructions)
|
||||
- [Getting start](#getting-start)
|
||||
- [Efficient intra-process communication:](#efficient-intra-process-communication)
|
||||
- [Example](#example)
|
||||
- [Manually loading multiple components into the same process](#manually-loading-multiple-components-into-the-same-process)
|
||||
- [Using a launch file](#using-a-launch-file)
|
||||
- [Limitations](#limitations)
|
||||
- [Use V4L2 backend](#use-v4l2-backend)
|
||||
- [Launch parameters](#launch-parameters)
|
||||
- [ROS2(Robot) vs Optical(Camera) Coordination Systems](#ros2robot-vs-opticalcamera-coordination-systems)
|
||||
- [Camera sensor structure](#camera-sensor-structure)
|
||||
- [TF from coordinate A to coordinate B:](#tf-from-coordinate-a-to-coordinate-b)
|
||||
- [Predefined presets](#predefined-presets)
|
||||
- [Depth work mode switch](#depth-work-mode-switch)
|
||||
- [Configuration of depth NFOV and WFOV modes](#configuration-of-depth-nfov-and-wfov-modes)
|
||||
- [All available service for camera control](#all-available-service-for-camera-control)
|
||||
- [All available topics](#all-available-topics)
|
||||
- [Network device enumeration](#network-device-enumeration)
|
||||
- [Multi-Camera](#multi-camera)
|
||||
- [Compressed Image](#compressed-image)
|
||||
- [Use hardware decoder to decode JPEG](#use-hardware-decoder-to-decode-jpeg)
|
||||
- [rockchip and Amlogic](#rockchip-and-amlogic)
|
||||
- [Nvidia Jetson](#nvidia-jetson)
|
||||
- [Check which profiles the camera supports](#check-which-profiles-the-camera-supports)
|
||||
- [Building a Debian Package](#building-a-debian-package)
|
||||
- [Preparing the Environment](#preparing-the-environment)
|
||||
- [Configuring ROS Dependencies](#configuring-ros-dependencies)
|
||||
- [Building the Package](#building-the-package)
|
||||
- [Supported Devices](#supported-devices)
|
||||
- [DDS Tuning](#dds-tuning)
|
||||
- [Frequently Asked Questions](#frequently-asked-questions)
|
||||
- [Unexpected Crash](#unexpected-crash)
|
||||
- [No Data Stream from Multiple Cameras](#no-data-stream-from-multiple-cameras)
|
||||
- [Additional Troubleshooting](#additional-troubleshooting)
|
||||
- [Why Are There So Many Launch Files?](#why-are-there-so-many-launch-files)
|
||||
- [Other useful links](#other-useful-links)
|
||||
- [License](#license)
|
||||
|
||||
## Installation Instructions
|
||||
|
||||
@@ -82,7 +201,7 @@ Get source code
|
||||
cd ~/ros2_ws/src
|
||||
git clone https://github.com/orbbec/OrbbecSDK_ROS2.git
|
||||
cd OrbbecSDK_ROS2
|
||||
git checkout OrbbecSDK_V2.X
|
||||
git checkout v2-main
|
||||
```
|
||||
|
||||
Install deb dependencies
|
||||
@@ -243,7 +362,7 @@ ros2 launch orbbec_camera gemini_intra_process_demo_launch.py
|
||||
To enable the V4L2 backend for the Gemini2 series cameras, follow these steps:
|
||||
|
||||
1. The Gemini2 series cameras support the V4L2 backend.
|
||||
2. Open the `config/OrbbecSDKConfig_v1.0.xml` file.
|
||||
2. Open the `config/OrbbecSDKConfig_v2.0.xml` file.
|
||||
3. Set the navigation option to `LinuxUVCBackend`.
|
||||
4. Change the backend setting to `V4L2`.
|
||||
|
||||
@@ -642,33 +761,27 @@ cd src/OrbbecSDK_ROS2/
|
||||
bash .make_deb.sh
|
||||
```
|
||||
|
||||
## Launch files
|
||||
## Supported Devices
|
||||
|
||||
| product serials | Firmware Version | **Firmware Version** |
|
||||
| :-------------: | :--------------: | :-------------------------: |
|
||||
Currently, the following devices are supported by the OrbbecSDK ROS2 Wrapper v2-main branch. More devices support will be added in the near future. If you can not find your device in the table below, try the [main](https://github.com/orbbec/OrbbecSDK_ROS2) branch.
|
||||
|
||||
For optimal performance, we strongly recommend updating to the latest firmware version. This ensures that you benefit from the most recent enhancements and bug fixes.
|
||||
|
||||
| Product List | Minimal Firmware Version | **Launch File** |
|
||||
| :-------------- | :--------------- | :-------------------------- |
|
||||
| Astra2 | 2.8.20 | astra2.launch.py |
|
||||
| Femto mega | 1.1.7/1.2.7 | femto_mega.launch.py |
|
||||
| Femto bolt | 1.0.6/1.0.9 | femto_bolt.launch.py |
|
||||
| Gemini2 | 1.4.60 /1.4.76 | gemini2.launch.py |
|
||||
| Gemini2L | 1.4.32 | gemini2L.launch.py |
|
||||
| Femto Mega | 1.1.7/1.2.7 | femto_mega.launch.py |
|
||||
| Femto Bolt | 1.0.6/1.0.9 | femto_bolt.launch.py |
|
||||
| Gemini 2 | 1.4.60 /1.4.76 | gemini2.launch.py |
|
||||
| Gemini 2 L | 1.4.32 | gemini2L.launch.py |
|
||||
| Gemini 335 | 1.2.20 | gemini_330_series.launch.py |
|
||||
| Gemini 335L | 1.2.20 | gemini_330_series.launch.py |
|
||||
| Gemini 335Lg | 1.3.46 | gemini_330_series.launch.py |
|
||||
| Gemini 336 | 1.2.20 | gemini_330_series.launch.py |
|
||||
| Gemini 336L | 1.2.20 | gemini_330_series.launch.py |
|
||||
|
||||
**All launch files are essentially similar, with the primary difference being the default values of the parameters set
|
||||
for different models
|
||||
within the same series. Differences in USB standards, such as USB 2.0 versus USB 3.0, may require adjustments to these
|
||||
parameters. If you
|
||||
encounter a startup failure, please carefully review the specification manual. Pay special attention to the resolution
|
||||
settings in the launch
|
||||
file, as well as other parameters, to ensure compatibility and optimal performance.**
|
||||
|
||||
## Product support
|
||||
|
||||
Please refer to the OrbbecSDK supported
|
||||
products: [Product Support](https://github.com/orbbec/OrbbecSDK?tab=readme-ov-file#product-support)
|
||||
All launch files are essentially similar, with the primary difference being the default values of the parameters set
|
||||
for different models within the same series. Differences in USB standards, such as USB 2.0 versus USB 3.0, may require adjustments to these parameters. If you encounter a startup failure, please carefully review the specification manual. Pay special attention to the resolution settings in the launch file, as well as other parameters, to ensure compatibility and optimal performance.
|
||||
|
||||
## DDS Tuning
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -1,23 +1,26 @@
|
||||
{
|
||||
"save_rgbir_params": {
|
||||
"time_domain": "device",
|
||||
"image_number": "100",
|
||||
"usb_ports": [
|
||||
"2-2",
|
||||
"2-3.3",
|
||||
"2-3.1",
|
||||
"2-3",
|
||||
"2-1"
|
||||
],
|
||||
"ir_topics": [
|
||||
"/G0_51/left_ir/image_raw",
|
||||
"/G1_54/left_ir/image_raw",
|
||||
"/G2_5Y/left_ir/image_raw",
|
||||
"/G3_47/left_ir/image_raw"
|
||||
"/G330_0/left_ir/image_raw",
|
||||
"/G330_1/left_ir/image_raw"
|
||||
],
|
||||
"left_ir_metadata_topic": [
|
||||
"/G330_0/left_ir/metadata",
|
||||
"/G330_1/left_ir/metadata"
|
||||
],
|
||||
"color_topics": [
|
||||
"/G0_51/color/image_raw",
|
||||
"/G1_54/color/image_raw",
|
||||
"/G2_5Y/color/image_raw",
|
||||
"/G3_47/color/image_raw"
|
||||
"/G330_0/color/image_raw",
|
||||
"/G330_1/color/image_raw"
|
||||
],
|
||||
"color_metadata_topic": [
|
||||
"/G330_0/color/metadata",
|
||||
"/G330_1/color/metadata"
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -76,11 +76,11 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||
DeclareLaunchArgument('use_hardware_time', default_value='false'),
|
||||
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
|
||||
DeclareLaunchArgument('align_mode', default_value='HW'),
|
||||
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
|
||||
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
|
||||
DeclareLaunchArgument('time_domain', default_value='device'),
|
||||
]
|
||||
|
||||
# Node configuration
|
||||
|
||||
@@ -78,10 +78,10 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||
DeclareLaunchArgument('enable_frame_sync', default_value='false'),
|
||||
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||
DeclareLaunchArgument('use_hardware_time', default_value='false'),
|
||||
DeclareLaunchArgument('align_mode', default_value='HW'),
|
||||
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
|
||||
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
|
||||
DeclareLaunchArgument('time_domain', default_value='device'),
|
||||
]
|
||||
|
||||
# Node configuration
|
||||
|
||||
@@ -84,11 +84,11 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
|
||||
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
|
||||
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||
DeclareLaunchArgument('use_hardware_time', default_value='true'),
|
||||
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
|
||||
DeclareLaunchArgument('align_mode', default_value='SW'),
|
||||
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
|
||||
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
|
||||
DeclareLaunchArgument('time_domain', default_value='device'),
|
||||
]
|
||||
|
||||
# Node configuration
|
||||
|
||||
@@ -90,11 +90,11 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
|
||||
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
|
||||
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||
DeclareLaunchArgument('use_hardware_time', default_value='false'),
|
||||
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
|
||||
DeclareLaunchArgument('align_mode', default_value='HW'),
|
||||
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
|
||||
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
|
||||
DeclareLaunchArgument('time_domain', default_value='device'),
|
||||
]
|
||||
|
||||
# Node configuration
|
||||
|
||||
@@ -89,12 +89,12 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
|
||||
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
|
||||
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||
DeclareLaunchArgument('use_hardware_time', default_value='true'),
|
||||
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
|
||||
DeclareLaunchArgument('align_mode', default_value='HW'),
|
||||
DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
|
||||
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
|
||||
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
|
||||
DeclareLaunchArgument('time_domain', default_value='device'),
|
||||
]
|
||||
|
||||
# Node configuration
|
||||
|
||||
@@ -89,13 +89,13 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
|
||||
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
|
||||
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||
DeclareLaunchArgument('use_hardware_time', default_value='true'),
|
||||
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
|
||||
DeclareLaunchArgument('align_mode', default_value='HW'),
|
||||
DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
|
||||
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
|
||||
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
|
||||
DeclareLaunchArgument('enable_noise_removal_filter', default_value='false'),
|
||||
DeclareLaunchArgument('time_domain', default_value='device'),
|
||||
]
|
||||
|
||||
# Node configuration
|
||||
|
||||
@@ -132,7 +132,6 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument('software_trigger_period', default_value='33'), # ms
|
||||
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
|
||||
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||
DeclareLaunchArgument('use_hardware_time', default_value='true'),
|
||||
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
|
||||
DeclareLaunchArgument('enable_decimation_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_hdr_merge', default_value='false'),
|
||||
@@ -175,6 +174,33 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
|
||||
DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
|
||||
DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'),
|
||||
DeclareLaunchArgument('interleave_ae_mode', default_value='laser'), # 'hdr' or 'laser'
|
||||
DeclareLaunchArgument('interleave_frame_enable', default_value='false'),
|
||||
DeclareLaunchArgument('interleave_skip_enable', default_value='false'),
|
||||
DeclareLaunchArgument('interleave_skip_index', default_value='1'), # 0:skip pattern ir 1: skip flood ir
|
||||
|
||||
DeclareLaunchArgument('hdr_index1_laser_control', default_value='1'),#interleave_hdr_param
|
||||
DeclareLaunchArgument('hdr_index1_depth_exposure', default_value='1'),
|
||||
DeclareLaunchArgument('hdr_index1_depth_gain', default_value='16'),
|
||||
DeclareLaunchArgument('hdr_index1_ir_brightness', default_value='20'),
|
||||
DeclareLaunchArgument('hdr_index1_ir_ae_max_exposure', default_value='2000'),
|
||||
DeclareLaunchArgument('hdr_index0_laser_control', default_value='1'),
|
||||
DeclareLaunchArgument('hdr_index0_depth_exposure', default_value='7500'),
|
||||
DeclareLaunchArgument('hdr_index0_depth_gain', default_value='16'),
|
||||
DeclareLaunchArgument('hdr_index0_ir_brightness', default_value='60'),
|
||||
DeclareLaunchArgument('hdr_index0_ir_ae_max_exposure', default_value='10000'),
|
||||
|
||||
DeclareLaunchArgument('laser_index1_laser_control', default_value='0'),#interleave_laser_param
|
||||
DeclareLaunchArgument('laser_index1_depth_exposure', default_value='3000'),
|
||||
DeclareLaunchArgument('laser_index1_depth_gain', default_value='16'),
|
||||
DeclareLaunchArgument('laser_index1_ir_brightness', default_value='60'),
|
||||
DeclareLaunchArgument('laser_index1_ir_ae_max_exposure', default_value='17000'),
|
||||
DeclareLaunchArgument('laser_index0_laser_control', default_value='1'),
|
||||
DeclareLaunchArgument('laser_index0_depth_exposure', default_value='3000'),
|
||||
DeclareLaunchArgument('laser_index0_depth_gain', default_value='16'),
|
||||
DeclareLaunchArgument('laser_index0_ir_brightness', default_value='60'),
|
||||
DeclareLaunchArgument('laser_index0_ir_ae_max_exposure', default_value='30000'),
|
||||
|
||||
]
|
||||
|
||||
def get_params(context, args):
|
||||
|
||||
@@ -128,7 +128,6 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument('software_trigger_period', default_value='33'), # ms
|
||||
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
|
||||
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||
DeclareLaunchArgument('use_hardware_time', default_value='true'),
|
||||
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
|
||||
DeclareLaunchArgument('enable_decimation_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_hdr_merge', default_value='false'),
|
||||
|
||||
@@ -66,6 +66,7 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||
DeclareLaunchArgument('time_domain', default_value='device'),
|
||||
]
|
||||
|
||||
# Node configuration
|
||||
|
||||
@@ -251,8 +251,7 @@ void OBCameraNode::setupDevices() {
|
||||
RCLCPP_INFO_STREAM(logger_, "Set depth work mode: " << depth_work_mode_);
|
||||
TRY_EXECUTE_BLOCK(device_->switchDepthWorkMode(depth_work_mode_.c_str()));
|
||||
}
|
||||
if (!sync_mode_str_.empty() && device_->isPropertySupported(OB_PROP_SYNC_SIGNAL_TRIGGER_OUT_BOOL,
|
||||
OB_PERMISSION_READ_WRITE)) {
|
||||
if (!sync_mode_str_.empty()) {
|
||||
auto sync_config = device_->getMultiDeviceSyncConfig();
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"Current sync mode: " << magic_enum::enum_name(sync_config.syncMode));
|
||||
@@ -329,12 +328,6 @@ void OBCameraNode::setupDevices() {
|
||||
device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_noise_removal_filter_);
|
||||
}
|
||||
|
||||
if (device_->isPropertySupported(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
|
||||
RCLCPP_INFO_STREAM(
|
||||
logger_, "Setting color auto exposure to " << (enable_color_auto_exposure_ ? "ON" : "OFF"));
|
||||
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL,
|
||||
enable_color_auto_exposure_);
|
||||
}
|
||||
if (device_->isPropertySupported(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, OB_PERMISSION_WRITE)) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Setting color auto white balance to "
|
||||
<< (enable_color_auto_white_balance_ ? "ON" : "OFF"));
|
||||
@@ -343,7 +336,6 @@ void OBCameraNode::setupDevices() {
|
||||
}
|
||||
if (color_exposure_ != -1 &&
|
||||
device_->isPropertySupported(OB_PROP_COLOR_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
|
||||
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, false);
|
||||
auto range = device_->getIntPropertyRange(OB_PROP_COLOR_EXPOSURE_INT);
|
||||
if (color_exposure_ < range.min || color_exposure_ > range.max) {
|
||||
RCLCPP_ERROR(logger_, "color exposure value is out of range[%d,%d], please check the value",
|
||||
@@ -355,7 +347,6 @@ void OBCameraNode::setupDevices() {
|
||||
}
|
||||
if (color_gain_ != -1 &&
|
||||
device_->isPropertySupported(OB_PROP_COLOR_GAIN_INT, OB_PERMISSION_WRITE)) {
|
||||
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, false);
|
||||
auto range = device_->getIntPropertyRange(OB_PROP_COLOR_GAIN_INT);
|
||||
if (color_gain_ < range.min || color_gain_ > range.max) {
|
||||
RCLCPP_ERROR(logger_, "color gain value is out of range[%d,%d], please check the value",
|
||||
@@ -365,6 +356,12 @@ void OBCameraNode::setupDevices() {
|
||||
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_GAIN_INT, color_gain_);
|
||||
}
|
||||
}
|
||||
if (device_->isPropertySupported(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
|
||||
RCLCPP_INFO_STREAM(
|
||||
logger_, "Setting color auto exposure to " << (enable_color_auto_exposure_ ? "ON" : "OFF"));
|
||||
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL,
|
||||
enable_color_auto_exposure_);
|
||||
}
|
||||
if (color_white_balance_ != -1 &&
|
||||
device_->isPropertySupported(OB_PROP_COLOR_WHITE_BALANCE_INT, OB_PERMISSION_WRITE)) {
|
||||
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, false);
|
||||
@@ -402,14 +399,8 @@ void OBCameraNode::setupDevices() {
|
||||
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_BRIGHTNESS_INT, ir_brightness_);
|
||||
}
|
||||
|
||||
if (device_->isPropertySupported(OB_PROP_IR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"Setting IR auto exposure to " << (enable_ir_auto_exposure_ ? "ON" : "OFF"));
|
||||
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
|
||||
}
|
||||
if (ir_exposure_ != -1 &&
|
||||
device_->isPropertySupported(OB_PROP_IR_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
|
||||
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, false);
|
||||
auto range = device_->getIntPropertyRange(OB_PROP_IR_EXPOSURE_INT);
|
||||
RCLCPP_ERROR(logger_, "ir exposure value is out of range[%d,%d], please check the value",
|
||||
range.min, range.max);
|
||||
@@ -422,7 +413,6 @@ void OBCameraNode::setupDevices() {
|
||||
}
|
||||
}
|
||||
if (ir_gain_ != -1 && device_->isPropertySupported(OB_PROP_IR_GAIN_INT, OB_PERMISSION_WRITE)) {
|
||||
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, false);
|
||||
auto range = device_->getIntPropertyRange(OB_PROP_IR_GAIN_INT);
|
||||
RCLCPP_ERROR(logger_, "ir gain value is out of range[%d,%d], please check the value", range.min,
|
||||
range.max);
|
||||
@@ -434,7 +424,11 @@ void OBCameraNode::setupDevices() {
|
||||
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_GAIN_INT, ir_gain_);
|
||||
}
|
||||
}
|
||||
|
||||
if (device_->isPropertySupported(OB_PROP_IR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"Setting IR auto exposure to " << (enable_ir_auto_exposure_ ? "ON" : "OFF"));
|
||||
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
|
||||
}
|
||||
if (device_->isPropertySupported(OB_PROP_IR_LONG_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"Setting IR long exposure to " << (enable_ir_long_exposure_ ? "ON" : "OFF"));
|
||||
@@ -442,28 +436,31 @@ void OBCameraNode::setupDevices() {
|
||||
}
|
||||
|
||||
if (device_->isPropertySupported(OB_PROP_DEPTH_MAX_DIFF_INT, OB_PERMISSION_WRITE)) {
|
||||
auto default_soft_filter_max_diff = device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
|
||||
RCLCPP_INFO_STREAM(logger_, "default_soft_filter_max_diff: " << default_soft_filter_max_diff);
|
||||
if (soft_filter_max_diff_ != -1 && default_soft_filter_max_diff != soft_filter_max_diff_) {
|
||||
device_->setIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT, soft_filter_max_diff_);
|
||||
auto new_soft_filter_max_diff = device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
|
||||
RCLCPP_INFO_STREAM(logger_, "after set soft_filter_max_diff: " << new_soft_filter_max_diff);
|
||||
auto default_noise_removal_filter_min_diff =
|
||||
device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
|
||||
RCLCPP_INFO_STREAM(logger_, "default_noise_removal_filter_min_diff: "
|
||||
<< default_noise_removal_filter_min_diff);
|
||||
if (noise_removal_filter_min_diff_ != -1 &&
|
||||
default_noise_removal_filter_min_diff != noise_removal_filter_min_diff_) {
|
||||
device_->setIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT, noise_removal_filter_min_diff_);
|
||||
auto new_noise_removal_filter_min_diff = device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
|
||||
RCLCPP_INFO_STREAM(logger_, "after set noise_removal_filter_min_diff: "
|
||||
<< new_noise_removal_filter_min_diff);
|
||||
}
|
||||
}
|
||||
|
||||
if (device_->isPropertySupported(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, OB_PERMISSION_WRITE)) {
|
||||
auto default_soft_filter_speckle_size =
|
||||
auto default_noise_removal_filter_max_size =
|
||||
device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"default_soft_filter_speckle_size: " << default_soft_filter_speckle_size);
|
||||
if (soft_filter_speckle_size_ != -1 &&
|
||||
default_soft_filter_speckle_size != soft_filter_speckle_size_) {
|
||||
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT,
|
||||
soft_filter_speckle_size_);
|
||||
auto new_soft_filter_speckle_size =
|
||||
RCLCPP_INFO_STREAM(logger_, "default_noise_removal_filter_max_size: "
|
||||
<< default_noise_removal_filter_max_size);
|
||||
if (noise_removal_filter_max_size_ != -1 &&
|
||||
default_noise_removal_filter_max_size != noise_removal_filter_max_size_) {
|
||||
device_->setIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, noise_removal_filter_max_size_);
|
||||
auto new_noise_removal_filter_max_size =
|
||||
device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"after set soft_filter_speckle_size: " << new_soft_filter_speckle_size);
|
||||
RCLCPP_INFO_STREAM(logger_, "after set noise_removal_filter_max_size: "
|
||||
<< new_noise_removal_filter_max_size);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -789,7 +786,6 @@ void OBCameraNode::updateImageConfig(const stream_index_pair &stream_index) {
|
||||
unit_step_size_[stream_index] = sizeof(uint16_t);
|
||||
}
|
||||
}
|
||||
|
||||
int OBCameraNode::init_interleave_hdr_param() {
|
||||
device_->setIntProperty(OB_PROP_FRAME_INTERLEAVE_CONFIG_INDEX_INT, 1);
|
||||
device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, hdr_index1_laser_control_);
|
||||
@@ -825,7 +821,6 @@ int OBCameraNode::init_interleave_laser_param() {
|
||||
device_->setIntProperty(OB_PROP_IR_AE_MAX_EXPOSURE_INT, laser_index0_ir_ae_max_exposure_);
|
||||
return 0;
|
||||
}
|
||||
|
||||
void OBCameraNode::startStreams() {
|
||||
if (pipeline_ != nullptr) {
|
||||
pipeline_.reset();
|
||||
@@ -834,20 +829,17 @@ void OBCameraNode::startStreams() {
|
||||
|
||||
try {
|
||||
setupPipelineConfig();
|
||||
|
||||
if (interleave_frame_enable_) {
|
||||
// set interleave mode
|
||||
if (interleave_ae_mode_ == "hdr") {
|
||||
RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to hdr");
|
||||
device_->loadFrameInterleave("hdr interleave");
|
||||
init_interleave_hdr_param();
|
||||
} else if (interleave_ae_mode_ == "laser") {
|
||||
RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to laser");
|
||||
device_->loadFrameInterleave("laser interleave");
|
||||
init_interleave_laser_param();
|
||||
} else {
|
||||
RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to nothing");
|
||||
}
|
||||
// set interleave mode
|
||||
if (interleave_ae_mode_ == "hdr" && interleave_frame_enable_) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to hdr");
|
||||
device_->loadFrameInterleave("hdr interleave");
|
||||
init_interleave_hdr_param();
|
||||
} else if (interleave_ae_mode_ == "laser" && interleave_frame_enable_) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to laser");
|
||||
device_->loadFrameInterleave("laser interleave");
|
||||
init_interleave_laser_param();
|
||||
} else {
|
||||
RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to nothing");
|
||||
}
|
||||
|
||||
pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) {
|
||||
@@ -971,7 +963,6 @@ void OBCameraNode::stopStreams() {
|
||||
}
|
||||
try {
|
||||
pipeline_->stop();
|
||||
|
||||
// disable interleave frame
|
||||
if ((interleave_ae_mode_ == "hdr") || (interleave_ae_mode_ == "laser")) {
|
||||
RCLCPP_INFO_STREAM(logger_, "current interleave_ae_mode_: " << interleave_ae_mode_);
|
||||
@@ -983,7 +974,6 @@ void OBCameraNode::stopStreams() {
|
||||
interleave_frame_enable_);
|
||||
}
|
||||
}
|
||||
|
||||
} catch (const ob::Error &e) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline: " << e.getMessage());
|
||||
} catch (...) {
|
||||
@@ -1286,6 +1276,9 @@ void OBCameraNode::getParameters() {
|
||||
if (isOpenNIDevice(pid)) {
|
||||
time_domain_ = "system";
|
||||
}
|
||||
if (time_domain_ == "global") {
|
||||
device_->enableGlobalTimestamp(true);
|
||||
}
|
||||
RCLCPP_INFO_STREAM(logger_, "current time domain: " << time_domain_);
|
||||
setAndGetNodeParameter<int>(frames_per_trigger_, "frames_per_trigger", 2);
|
||||
long software_trigger_period = 33;
|
||||
|
||||
@@ -75,7 +75,7 @@ OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
|
||||
: Node("orbbec_camera_node", "/", node_options),
|
||||
node_options_(node_options),
|
||||
config_path_(ament_index_cpp::get_package_share_directory("orbbec_camera") +
|
||||
"/config/OrbbecSDKConfig_v1.0.xml"),
|
||||
"/config/OrbbecSDKConfig_v2.0.xml"),
|
||||
logger_(this->get_logger()),
|
||||
extension_path_(ament_index_cpp::get_package_prefix("orbbec_camera") + "/lib/extensions") {
|
||||
init();
|
||||
@@ -86,7 +86,7 @@ OBCameraNodeDriver::OBCameraNodeDriver(const std::string &node_name, const std::
|
||||
: Node(node_name, ns, node_options),
|
||||
node_options_(node_options),
|
||||
config_path_(ament_index_cpp::get_package_share_directory("orbbec_camera") +
|
||||
"/config/OrbbecSDKConfig_v1.0.xml"),
|
||||
"/config/OrbbecSDKConfig_v2.0.xml"),
|
||||
logger_(this->get_logger()),
|
||||
extension_path_(ament_index_cpp::get_package_prefix("orbbec_camera") + "/lib/extensions") {
|
||||
init();
|
||||
|
||||
@@ -479,7 +479,6 @@ void OBCameraNode::setLaserEnableCallback(
|
||||
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
|
||||
(void)request_header;
|
||||
(void)response;
|
||||
auto device_info = device_->getDeviceInfo();
|
||||
int laser_enable = request->data ? 1 : 0;
|
||||
try {
|
||||
if (device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) {
|
||||
@@ -812,6 +811,7 @@ void OBCameraNode::setRESETTimestampCallback(
|
||||
(void)request;
|
||||
try {
|
||||
device_->setBoolProperty(OB_PROP_TIMER_RESET_TRIGGER_OUT_ENABLE_BOOL, true);
|
||||
device_->setBoolProperty(OB_PROP_TIMER_RESET_SIGNAL_BOOL, true);
|
||||
response->success = true;
|
||||
} catch (const ob::Error& e) {
|
||||
response->message = e.getMessage();
|
||||
|
||||
@@ -17,7 +17,7 @@
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<MultiCameraSubscriber>();
|
||||
auto node = std::make_shared<orbbec_camera::tools::MultiCameraSubscriber>();
|
||||
rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 20);
|
||||
executor.add_node(node);
|
||||
executor.spin();
|
||||
|
||||
@@ -3,11 +3,18 @@
|
||||
|
||||
#include <orbbec_camera/ob_camera_node_driver.h>
|
||||
#include <orbbec_camera/utils.h>
|
||||
#include "orbbec_camera_msgs/msg/metadata.hpp"
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
#include <std_msgs/msg/bool.hpp>
|
||||
#include <filesystem>
|
||||
namespace orbbec_camera {
|
||||
namespace tools {
|
||||
struct ImageMetadata {
|
||||
std::vector<std::vector<std::string>> exposure_buffs;
|
||||
std::vector<std::vector<std::string>> gain_buffs;
|
||||
};
|
||||
|
||||
class MultiCameraSubscriber : public rclcpp::Node {
|
||||
public:
|
||||
@@ -61,7 +68,8 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
}
|
||||
|
||||
private:
|
||||
std::mutex buffer_mutex_;
|
||||
std::mutex image_mutex_;
|
||||
std::mutex meta_mutex_;
|
||||
void params_init() {
|
||||
std::ifstream file(
|
||||
"install/orbbec_camera/share/orbbec_camera/config/tools/multisavergbir/"
|
||||
@@ -72,10 +80,16 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
}
|
||||
nlohmann::json json_data;
|
||||
file >> json_data;
|
||||
time_domain_= json_data["save_rgbir_params"]["time_domain"].get<std::string>();
|
||||
time_domain_ =(time_domain_ == "device") ? "_d" : (time_domain_ == "global" ? "_g" : "_unknown");
|
||||
image_number_ = json_data["save_rgbir_params"]["image_number"].get<std::string>();
|
||||
usb_params_ = json_data["save_rgbir_params"]["usb_ports"].get<std::vector<std::string>>();
|
||||
left_ir_metadata_topic_ =
|
||||
json_data["save_rgbir_params"]["left_ir_metadata_topic"].get<std::vector<std::string>>();
|
||||
ir_topics_ = json_data["save_rgbir_params"]["ir_topics"].get<std::vector<std::string>>();
|
||||
color_topics_ = json_data["save_rgbir_params"]["color_topics"].get<std::vector<std::string>>();
|
||||
color_metadata_topic_ =
|
||||
json_data["save_rgbir_params"]["color_metadata_topic"].get<std::vector<std::string>>();
|
||||
}
|
||||
void topic_init() {
|
||||
auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
|
||||
@@ -84,8 +98,12 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
for (size_t i = 0; i < ir_topics_.size(); ++i) {
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
|
||||
"ir_topic: " << ir_topics_[i]);
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
|
||||
"left_ir_metadata_topic_: " << left_ir_metadata_topic_[i]);
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
|
||||
"color_topic: " << color_topics_[i]);
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
|
||||
"color_metadata_topic_: " << color_metadata_topic_[i]);
|
||||
|
||||
rclcpp::SubscriptionOptions ir_sub_options;
|
||||
ir_sub_options.callback_group = reentrant_callback_group_;
|
||||
@@ -100,6 +118,12 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
},
|
||||
ir_sub_options);
|
||||
|
||||
auto ir_metadata_sub = this->create_subscription<orbbec_camera_msgs::msg::Metadata>(
|
||||
left_ir_metadata_topic_[i], custom_qos,
|
||||
[this, i](std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg) {
|
||||
this->ir_meta_Callback(msg, i);
|
||||
});
|
||||
|
||||
auto color_sub = this->create_subscription<sensor_msgs::msg::Image>(
|
||||
color_topics_[i], custom_qos,
|
||||
[this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
|
||||
@@ -107,8 +131,16 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
},
|
||||
color_sub_options);
|
||||
|
||||
auto color_metadata_sub = this->create_subscription<orbbec_camera_msgs::msg::Metadata>(
|
||||
color_metadata_topic_[i], custom_qos,
|
||||
[this, i](std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg) {
|
||||
this->color_meta_Callback(msg, i);
|
||||
});
|
||||
|
||||
ir_subscribers_.push_back(ir_sub);
|
||||
ir_meta_subscribers_.push_back(ir_metadata_sub);
|
||||
color_subscribers_.push_back(color_sub);
|
||||
color_meta_subscribers_.push_back(color_metadata_sub);
|
||||
|
||||
ir_image_buffers_.resize(ir_topics_.size());
|
||||
color_image_buffers_.resize(ir_topics_.size());
|
||||
@@ -116,6 +148,10 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
color_current_timestamp_buffers_.resize(ir_topics_.size());
|
||||
ir_timestamp_buffers_.resize(ir_topics_.size());
|
||||
color_timestamp_buffers_.resize(ir_topics_.size());
|
||||
left_ir_metadata_.exposure_buffs.resize(ir_topics_.size());
|
||||
left_ir_metadata_.gain_buffs.resize(ir_topics_.size());
|
||||
color_metadata_.exposure_buffs.resize(ir_topics_.size());
|
||||
color_metadata_.gain_buffs.resize(ir_topics_.size());
|
||||
callback_called_ = std::vector<bool>(ir_topics_.size(), false);
|
||||
}
|
||||
}
|
||||
@@ -163,17 +199,21 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
auto &color_images = color_image_buffers_[index];
|
||||
auto &color_current_timestamps = color_current_timestamp_buffers_[index];
|
||||
auto &color_timestamps = color_timestamp_buffers_[index];
|
||||
auto &left_ir_meta_exposure = left_ir_metadata_.exposure_buffs[index];
|
||||
auto &left_ir_meta_gain = left_ir_metadata_.gain_buffs[index];
|
||||
auto &color_meta_exposure = color_metadata_.exposure_buffs[index];
|
||||
auto &color_meta_gain = color_metadata_.gain_buffs[index];
|
||||
callback_called_[index] = true;
|
||||
if (ir_images.size() < static_cast<size_t>(std::stoi(image_number_)) ||
|
||||
color_images.size() < static_cast<size_t>(std::stoi(image_number_))) {
|
||||
return;
|
||||
}
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "jjjjj1");
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "index:"<<index);
|
||||
auto usb_iter = usb_index_map_.find(usb_numbers_[index]);
|
||||
auto serial_iter = serial_numbers_.find(usb_numbers_[index]);
|
||||
int usb_index = usb_iter->second;
|
||||
if (serial_iter == serial_numbers_.end()) {
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "jjjjj2");
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "serial_iter is empty");
|
||||
return;
|
||||
}
|
||||
std::string serial_index = serial_iter->second;
|
||||
@@ -181,46 +221,42 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
for (size_t i = 0; i < static_cast<size_t>(std::stoi(image_number_)); i++) {
|
||||
std::string folder = generateFolderName(serial_index, usb_index);
|
||||
std::string ir_filename = folder + "/ir#left_SN" + serial_index + "_Index" +
|
||||
std::to_string(usb_index) + "_d" + ir_current_timestamps[i] + "_f" +
|
||||
std::to_string(i) + "_s" + ir_timestamps[i] + "_.jpg";
|
||||
std::to_string(usb_index) + time_domain_ + ir_current_timestamps[i] + "_f" +
|
||||
std::to_string(i) + "_s" + ir_timestamps[i] + "_e" +
|
||||
left_ir_meta_exposure[i] + "_g" + left_ir_meta_gain[i] + "_.jpg";
|
||||
if (ir_images[i].empty()) {
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "over ");
|
||||
// rclcpp::shutdown();
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "over ");
|
||||
continue;
|
||||
}
|
||||
|
||||
cv::imwrite(ir_filename, ir_images[i]);
|
||||
// RCLCPP_INFO(this->get_logger(), "Saved IR image to: %s", ir_filename.c_str());
|
||||
|
||||
std::string color_filename = folder + "/color_SN" + serial_index + "_Index" +
|
||||
std::to_string(usb_index) + "_d" + color_current_timestamps[i] +
|
||||
"_f" + std::to_string(i) + "_s" + color_timestamps[i] + "_.jpg";
|
||||
if (ir_images[i].empty()) {
|
||||
// rclcpp::shutdown();
|
||||
std::to_string(usb_index) + time_domain_ + color_current_timestamps[i] +
|
||||
"_f" + std::to_string(i) + "_s" + color_timestamps[i] + "_e" +
|
||||
color_meta_exposure[i] + "_g" + color_meta_gain[i] +"_.jpg";
|
||||
if (color_images[i].empty()) {
|
||||
continue;
|
||||
}
|
||||
cv::imwrite(color_filename, color_images[i]);
|
||||
// RCLCPP_INFO(this->get_logger(), "Saved Color image to: %s", color_filename.c_str());
|
||||
}
|
||||
|
||||
ir_images.clear();
|
||||
color_images.clear();
|
||||
ir_current_timestamps.clear();
|
||||
color_current_timestamps.clear();
|
||||
ir_timestamps.clear();
|
||||
color_timestamps.clear();
|
||||
ir_image_buffers_[index].clear();
|
||||
ir_current_timestamp_buffers_[index].clear();
|
||||
ir_timestamp_buffers_[index].clear();
|
||||
color_image_buffers_[index].clear();
|
||||
color_current_timestamp_buffers_[index].clear();
|
||||
color_timestamp_buffers_[index].clear();
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"),
|
||||
left_ir_metadata_.exposure_buffs[index].clear();
|
||||
left_ir_metadata_.gain_buffs[index].clear();
|
||||
color_metadata_.exposure_buffs[index].clear();
|
||||
color_metadata_.gain_buffs[index].clear();
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
|
||||
"callback_called_ " << index << ":" << callback_called_[index]);
|
||||
bool all_true =
|
||||
std::all_of(callback_called_.begin(), callback_called_.end(), [](bool v) { return v; });
|
||||
if (all_true) {
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "over ");
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "over ");
|
||||
ir_image_buffers_.clear();
|
||||
ir_current_timestamp_buffers_.clear();
|
||||
ir_timestamp_buffers_.clear();
|
||||
@@ -234,10 +270,10 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
void controlCaptureCallback(const std_msgs::msg::Bool::SharedPtr msg) {
|
||||
is_saving_images_ = msg->data;
|
||||
topic_init();
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "jjjj " << is_saving_images_);
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "jjjj " << is_saving_images_);
|
||||
}
|
||||
void irCallback(std::shared_ptr<const sensor_msgs::msg::Image> image, size_t index) {
|
||||
std::lock_guard<std::mutex> lock(buffer_mutex_);
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
if (!callback_called_[index] && is_saving_images_) {
|
||||
cv::Mat ir_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
|
||||
std::string current_timestamp_ir = getCurrentTimestamp(image);
|
||||
@@ -246,7 +282,7 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
ir_current_timestamp_buffers_[index].push_back(current_timestamp_ir);
|
||||
ir_timestamp_buffers_[index].push_back(timestamp_ir);
|
||||
ir_resolution_ = std::to_string(image->width) + "x" + std::to_string(image->height);
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"),
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
|
||||
":ir: " << index << ":" << ir_image_buffers_[index].size());
|
||||
if (ir_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_)) &&
|
||||
color_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_))) {
|
||||
@@ -254,9 +290,8 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void colorCallback(std::shared_ptr<const sensor_msgs::msg::Image> image, size_t index) {
|
||||
std::lock_guard<std::mutex> lock(buffer_mutex_);
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
if (!callback_called_[index] && is_saving_images_) {
|
||||
cv::Mat color_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
|
||||
cv::Mat corrected_image;
|
||||
@@ -267,7 +302,7 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
color_current_timestamp_buffers_[index].push_back(current_timestamp_color);
|
||||
color_timestamp_buffers_[index].push_back(timestamp_color);
|
||||
color_resolution_ = std::to_string(image->width) + "x" + std::to_string(image->height);
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"),
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
|
||||
":color: " << index << ":" << color_image_buffers_[index].size());
|
||||
if (ir_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_)) &&
|
||||
color_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_))) {
|
||||
@@ -276,7 +311,26 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
}
|
||||
}
|
||||
|
||||
void ir_meta_Callback(std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg,
|
||||
size_t index) {
|
||||
std::lock_guard<std::mutex> lock(meta_mutex_);
|
||||
nlohmann::json json_data = nlohmann::json::parse(msg->json_data);
|
||||
left_ir_metadata_.exposure_buffs[index].push_back(json_data["exposure"].dump());
|
||||
left_ir_metadata_.gain_buffs[index].push_back(json_data["gain"].dump());
|
||||
}
|
||||
void color_meta_Callback(std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg,
|
||||
size_t index) {
|
||||
std::lock_guard<std::mutex> lock(meta_mutex_);
|
||||
nlohmann::json json_data = nlohmann::json::parse(msg->json_data);
|
||||
color_metadata_.exposure_buffs[index].push_back(json_data["exposure"].dump());
|
||||
color_metadata_.gain_buffs[index].push_back(json_data["gain"].dump());
|
||||
}
|
||||
|
||||
rclcpp::CallbackGroup::SharedPtr reentrant_callback_group_;
|
||||
std::vector<rclcpp::Subscription<orbbec_camera_msgs::msg::Metadata>::SharedPtr>
|
||||
ir_meta_subscribers_;
|
||||
std::vector<rclcpp::Subscription<orbbec_camera_msgs::msg::Metadata>::SharedPtr>
|
||||
color_meta_subscribers_;
|
||||
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> ir_subscribers_;
|
||||
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> color_subscribers_;
|
||||
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr capture_control_sub_;
|
||||
@@ -289,9 +343,12 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
size_t count = 0;
|
||||
|
||||
std::vector<std::string> usb_params_;
|
||||
std::vector<std::string> left_ir_metadata_topic_;
|
||||
std::vector<std::string> color_metadata_topic_;
|
||||
std::vector<std::string> ir_topics_;
|
||||
std::vector<std::string> color_topics_;
|
||||
std::string image_number_;
|
||||
std::string time_domain_;
|
||||
|
||||
std::vector<std::vector<cv::Mat>> ir_image_buffers_;
|
||||
std::vector<std::vector<cv::Mat>> color_image_buffers_;
|
||||
@@ -307,4 +364,9 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
std::string currenttimes_;
|
||||
|
||||
bool is_saving_images_ = false;
|
||||
|
||||
ImageMetadata left_ir_metadata_ = ImageMetadata();
|
||||
ImageMetadata color_metadata_ = ImageMetadata();
|
||||
};
|
||||
} // namespace tools
|
||||
} // namespace orbbec_camera
|
||||
|
||||
Reference in New Issue
Block a user