Added H26x decoding node for femto_mega

README.MD

@@ -1,4 +1,5 @@
 # Orbbec ROS2 SDK
+
  [![stable](http://badges.github.io/stability-badges/dist/stable.svg)](http://github.com/badges/stability-badges) ![version](https://img.shields.io/badge/version-1.5.11-green)
 
 Orbbec SDK ROS 2 is a wrapper for the Orbbec 3D camera that provides seamless integration with the ROS 2 environment. It
@@ -7,6 +8,7 @@ supports ROS 2 Foxy, Humble, and Jazzy distributions.
 ## Table of Contents
 
 <!-- TOC -->
+
 * [Orbbec ROS2 SDK](#orbbec-ros2-sdk)
   * [Table of Contents](#table-of-contents)
   * [Installation Instructions](#installation-instructions)
@@ -44,6 +46,7 @@ supports ROS 2 Foxy, Humble, and Jazzy distributions.
     * [Why Are There So Many Launch Files?](#why-are-there-so-many-launch-files)
   * [Other useful links](#other-useful-links)
   * [License](#license)
+
 <!-- TOC -->
 
 ## Installation Instructions
@@ -193,7 +196,7 @@ ros2 service call /camera/save_point_cloud std_srvs/srv/Empty "{}"
 ```
 
 ## Efficient intra-process communication:
-  
+
 Our ROS2 Wrapper node supports zero-copy communications if loaded in the same process as a subscriber node. This can reduce copy times on image/pointcloud topics, especially with big frame resolutions and high FPS.
 
 You will need to launch a component container and launch our node as a component together with other component nodes. Further details on "Composing multiple nodes in a single process" can be found [here](https://docs.ros.org/en/rolling/Tutorials/Composition.html).
@@ -201,16 +204,20 @@ You will need to launch a component container and launch our node as a component
 Further details on efficient intra-process communication can be found [here](https://docs.ros.org/en/humble/Tutorials/Intra-Process-Communication.html#efficient-intra-process-communication).
 
 ### Example
+
 #### Manually loading multiple components into the same process
+
 * Start the component:
+
   ```bash
   ros2 run rclcpp_components component_container
   ```
-
 * Add the wrapper:
+
   ```bash
   ros2 component load /ComponentManager orbbec_camera orbbec_camera::OBCameraNodeDriver -e use_intra_process_comms:=true
   ```
+
   Load other component nodes (consumers of the wrapper topics) in the same way.
 
 #### Using a launch file
@@ -225,6 +232,7 @@ ros2 launch orbbec_camera gemini_intra_process_demo_launch.py
 * Compressed images using `image_transport` will be disabled as this isn't supported with intra-process communication
 
 ## Use V4L2 backend
+
 To enable the V4L2 backend for the Gemini2 series cameras, follow these steps:
 
 1. The Gemini2 series cameras support the V4L2 backend.
@@ -307,19 +315,18 @@ The following are the launch parameters available:
   attempt to reset the camera up to three times. This setting aims to prevent USB 3.0 devices from being incorrectly
   recognized as USB 2.0. It is recommended to set this parameter to `false` when using a USB 2.0 connection to avoid
   unnecessary resets.
-
-- `enable_3d_reconstruction_mode`: Enables 3D reconstruction mode. Default is `false`. When set to `true`, the camera 
-laser operates in on-off mode, capturing IR images (laser off) for VSLAM localization and depth images (laser on) for point cloud computation.
+- `enable_3d_reconstruction_mode`: Enables 3D reconstruction mode. Default is `false`. When set to `true`, the camera
+  laser operates in on-off mode, capturing IR images (laser off) for VSLAM localization and depth images (laser on) for point cloud computation.
 - `tf_publish_rate`: The rate at which the camera publishes dynamic transforms. The default value is `0.0`, which means static transforms are published.
 - `time_domain`: The frame time domain, string type, can be `device`, `global`, or `system`. `device` means using the hardware timestamp from the camera,
-`system` means using the timestamp when the PC received the first packet of data or frame, and `global` is used for synchronized time across multiple
-devices, aligning data from different sources to a common time base.
+  `system` means using the timestamp when the PC received the first packet of data or frame, and `global` is used for synchronized time across multiple
+  devices, aligning data from different sources to a common time base.
 - `enable_sync_host_time`: Enables synchronization of the host time with the camera time. The default value is `true`, if
-use global time, set to `false`. Some old devices may not support this feature.
+  use global time, set to `false`. Some old devices may not support this feature.
 - `config_file_path`: The path to the YAML configuration file. The default value is `""`. If the configuration file is not specified,
-the default parameters from the launch file will be used. If you want to use a custom configuration file, please refer to `gemini_330_series.launch.py`.
-  `enable_heartbeat` enables the heartbeat function, which is set to `false` by default. If set to `true`, the camera node will send heartbeat signals to 
-the firmware, and if hardware logging is desired, it should also be set to `true`.
+  the default parameters from the launch file will be used. If you want to use a custom configuration file, please refer to `gemini_330_series.launch.py`.
+  `enable_heartbeat` enables the heartbeat function, which is set to `false` by default. If set to `true`, the camera node will send heartbeat signals to
+  the firmware, and if hardware logging is desired, it should also be set to `true`.
 - `log_level` : SDK log level, the default value is `info`, the optional values are `debug`, `info`, `warn`, `error`, `fatal`.
 - `enable_color_undistortion`: Enables color undistortion, the default value is `false`. Note that our color cameras exhibit minimal distortion, and typically, undistortion is not necessary.
 - `color_brightness`: Color brightness.
@@ -333,14 +340,14 @@ these settings.*
 
 ## Predefined presets
 
-| Preset         | Features                                                                                                                                                                                                      | Recommended use cases                                                                                                                                         |
-|----------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------|
-| Default        | - Best visual perception<br/>- Overall good performance in accuracy, fill rate, tiny objects, etc.                                                                                                            | - Generic<br>- Robotics                                                                                                                                       |
-| Hand           | - Clear hand and finger edges                                                                                                                                                                                 | - Gesture recognition                                                                                                                                         |
-| High Accuracy  | - Depth of high confidence<br>- Barely noise depth values<br>- Lower fill rate                                                                                                                                | - Collision avoidance<br>- Object scanning                                                                                                                    |
-| High Density   | - Higher fill rate<br>- More tiny objects<br>- May suffer from noise depth values                                                                                                                             | - Object recognition<br>- Pick & place<br>- Foreground & background animation                                                                                 |
-| Medium Density | - Balanced performance in fill rate and accuracy<br>- In comparison to Default: lower fill rate, better edge quality                                                                                          | - Generic and alternative to Default                                                                                                                          |
-| Custom         | - User defined Preset<br>- Derived from Presets above, with customized modifications, e.g. a new configuration for the post-processing pipeline, modified mean intensity set point of depth AE function, etc. | - Better depth performance achieved using customized configurations in comparison to using predefined presets<br>- For well-established custom configurations |
+| Preset         | Features                                                                                                                                                                                                          | Recommended use cases                                                                                                                                             |
+| -------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| Default        | - Best visual perception``- Overall good performance in accuracy, fill rate, tiny objects, etc.                                                                                                            | - Generic`<br>`- Robotics                                                                                                                                       |
+| Hand           | - Clear hand and finger edges                                                                                                                                                                                     | - Gesture recognition                                                                                                                                             |
+| High Accuracy  | - Depth of high confidence`<br>`- Barely noise depth values`<br>`- Lower fill rate                                                                                                                            | - Collision avoidance`<br>`- Object scanning                                                                                                                    |
+| High Density   | - Higher fill rate`<br>`- More tiny objects`<br>`- May suffer from noise depth values                                                                                                                         | - Object recognition`<br>`- Pick & place`<br>`- Foreground & background animation                                                                             |
+| Medium Density | - Balanced performance in fill rate and accuracy`<br>`- In comparison to Default: lower fill rate, better edge quality                                                                                          | - Generic and alternative to Default                                                                                                                              |
+| Custom         | - User defined Preset`<br>`- Derived from Presets above, with customized modifications, e.g. a new configuration for the post-processing pipeline, modified mean intensity set point of depth AE function, etc. | - Better depth performance achieved using customized configurations in comparison to using predefined presets`<br>`- For well-established custom configurations |
 
 Choose the appropriate preset name based on your specific use case and set it as the value for the `device_preset`
 parameter.
@@ -435,7 +442,7 @@ to `true` in the stream that corresponds to the argument of the launch file.
 - `/camera/ir/camera_info`: The IR camera info.
 - `/camera/ir/image_raw`: The IR stream image
 - `/camera/accel/sample`: Acceleration data stream `enable_sync_output_accel_gyro`turned off，`enable_accel`turned on
-- `/camera/gyro/sample`: Gyroscope data stream，enable_sync_output_accel_gyro`turned off，`enable_gyro`turned on
+- `/camera/gyro/sample`: Gyroscope data stream，enable_sync_output_accel_gyro `turned off，`enable_gyro`turned on
 - `camera/gyro_accel/sample`: Synchronized data stream of acceleration and gyroscope，`enable_sync_output_accel_gyro`
   turned on
 - `/diagnostics`: The diagnostic information of the camera, Currently, the diagnostic information only includes the
@@ -522,9 +529,11 @@ ros2 launch orbbec_camera multi_camera.launch.py
 ```
 
 ## Compressed Image
+
 You can use `image_transport` to compress the image using `jpeg`. Below is an example of how to use it:
 
 To access the compressed color image, you can use the following command:
+
 ```bash
 ros2 topic echo /camera/color/image_raw/compressed --no-arr
 ```
@@ -550,6 +559,28 @@ Open `CMakeLists.txt` and set `USE_NV_HW_DECODER` to `ON`.
 ros2 run orbbec_camera list_camera_profile_mode_node
 ```
 
+## Tools for decoding H264/265 using FFmpeg
+
+For Femto Mega that can output H264/H265 encoding format, you can use the mega_h26x_decode_node node to implement the H264/H265 decoding function.
+The decoding function of the mega_h26x_decode_node node is implemented through FFmpeg.
+
+* First start the Femto Mega camera and set the color encoding format to H264 or H265:
+
+```bash
+ros2 launch orbbec_camera femto_mega.launch.py color_format:=H264
+```
+
+* Start the decoding node:
+
+```bash
+ros2 run orbbec_camera mega_h26x_decode_node
+```
+
+Available topics for the mega_h26x_decode_node node:
+
+* /camera/color/h26x_encoded_data : Femto Mega outputs raw color H264/H265 encoded data
+* /camera/color/h26x_decoder/image_raw : RGB image data decoded by the mega_h26x_decode_node node
+
 ## Building a Debian Package
 
 ### Preparing the Environment
@@ -599,7 +630,7 @@ bash .make_deb.sh
 ## Launch files
 
 | product serials                       | launch file                 |
-|---------------------------------------|-----------------------------|
+| ------------------------------------- | --------------------------- |
 | astra+                                | astra_adv.launch.py         |
 | astra mini /astra mini pro /astra pro | astra.launch.py             |
 | astra mini pro s                      | astra.launch.py             |
@@ -706,7 +737,6 @@ net.core.rmem_default=2147483647
 
 If you use Fast DDS, you can refer to the [Fast DDS Configuration](./docs/fastdds_tuning.md) file.
 
-
 ## Frequently Asked Questions
 
 ### Unexpected Crash
@@ -717,24 +747,29 @@ Please send this log to the support team or submit it to a GitHub issue for furt
 ### No Data Stream from Multiple Cameras
 
 **Insufficient Power Supply**:
+
 - Ensure that each camera is connected to a separate hub.
 - Use a powered hub to provide sufficient power to each camera.
 
 **High Resolution**:
+
 - Try lowering the resolution to resolve data stream issues.
 
 **Increase usbfs_memory_mb Value**:
-- Increase the `usbfs_memory_mb` value to 128MB (this is a reference value and can be adjusted based on your system’s needs) 
-by running the following command:
+
+- Increase the `usbfs_memory_mb` value to 128MB (this is a reference value and can be adjusted based on your system’s needs)
+  by running the following command:
+
 ```bash
     echo 128 | sudo tee /sys/module/usbcore/parameters/usbfs_memory_mb
 ```
+
 - To make this change permanent, check [this link](https://github.com/OpenKinect/libfreenect2/issues/807).
 
 ### Additional Troubleshooting
 
-- If you encounter other issues, set the `log_level` parameter to `debug`. This will generate an SDK log file in the running directory: `Log/OrbbecSDK.log.txt`. 
-Please provide this file to the support team for further assistance.
+- If you encounter other issues, set the `log_level` parameter to `debug`. This will generate an SDK log file in the running directory: `Log/OrbbecSDK.log.txt`.
+  Please provide this file to the support team for further assistance.
 - If firmware logs are required, set `enable_heartbeat` to `true` to activate this feature.
 
 ### Why Are There So Many Launch Files?

README_CN.MD

@@ -1,7 +1,5 @@
 # orbbec_camera
 
-## [![stable](http://badges.github.io/stability-badges/dist/stable.svg)](http://github.com/badges/stability-badges) ![version](https://img.shields.io/badge/version-1.5.2-green)
-
 **请注意：本文档为机器翻译，仅供参考。翻译可能存在遗漏或错误，且不会及时更新。请以英文原文为准。**
 
 OrbbecSDK ROS 2 是一个用于 Orbbec 3D 相机的 ROS 2 环境无缝集成的包装器。它支持 ROS 2 Foxy、Galactic 和 Humble 版本。
@@ -357,6 +355,27 @@ IR 的帧率和分辨率必须与深度一致。不同模式与分辨率的对
 ros2 run orbbec_camera list_camera_profile_mode_node
 ```
 
+## 使用FFmpeg解码H264/265的工具
+
+对于Femto Mega这种能够输出H264/H265编码格式的，能够使用mega_h26x_decode_node节点来实现H264/H265解码功能。
+mega_h26x_decode_node节点的解码功能是通过FFmpeg来实现的。
+
+* 首先启动Femto Mega相机并将color的编码格式设置为H264或者H265：
+
+  ```bash
+  ros2 launch orbbec_camera femto_mega.launch.py color_format:=H264
+  ```
+* 启动解码节点：
+
+  ```bash
+  ros2 run orbbec_camera mega_h26x_decode_node
+  ```
+
+mega_h26x_decode_node节点可用的话题：
+
+* /camera/color/h26x_encoded_data ：Femto Mega输出原始color的H264/H265编码数据
+* /camera/color/h26x_decoder/image_raw ：经过mega_h26x_decode_node节点解码后的RGB图像数据
+
 ## 构建 Debian 包
 
 ### 准备环境
@@ -403,63 +422,63 @@ bash .make_deb.sh
 
 ## 启动文件
 
-| 产品系列          | 启动文件                    |
-| ----------------- | --------------------------- |
-| astra+            | astra_adv.launch.py         |
+| 产品系列                              | 启动文件                    |
+| ------------------------------------- | --------------------------- |
+| astra+                                | astra_adv.launch.py         |
 | astra mini /astra mini pro /astra pro | astra.launch.py             |
-| astra mini pro s  | astra.launch.py             |
-| astra2            | astra2.launch.py            |
-| astra stereo s    | stereo_s_u3.launch.py       |
-| astra pro2        | astra_pro2.launch.py        |
-| dabai             | dabai.launch.py             |
-| dabai d1          | dabai_d1.launch.py          |
-| dabai dcw         | dabai_dcw.launch.py         |
-| dabai dw          | dabai_dw.launch.py          |
-| dabai pro         | dabai_pro.launch.py         |
-| deeya             | deeya.launch.py             |
-| femto /femto w    | femto.launch.py             |
-| femto mega        | femto_mega.launch.py        |
-| femto bolt        | femto_bolt.launch.py        |
-| gemini            | gemini.launch.py            |
-| gemini            | gemini.launch.py            |
-| gemini2 / dabai DCL | gemini2.launch.py           |
-| gemini2L          | gemini2L.launch.py          |
-| gemini e          | gemini_e.launch.py          |
-| gemini e lite     | gemini_e_lite.launch.py     |
-| dabai max         | dabai_max.launch.py         |
-| dabai max pro     | dabai_max_pro.launch.py     |
-| gemini uw         | gemini_uw.launch.py         |
-| dabai dcw2        | dabai_dcw2.launch.py        |
-| dabai dw2         | dabai_dw2.launch.py         |
-| gemini ew         | gemini_ew.launch.py         |
-| gemini ew lite    | gemini_ew_lite.launch.py    |
-| gemini 330 系列   | gemini_330_series.launch.py |
+| astra mini pro s                      | astra.launch.py             |
+| astra2                                | astra2.launch.py            |
+| astra stereo s                        | stereo_s_u3.launch.py       |
+| astra pro2                            | astra_pro2.launch.py        |
+| dabai                                 | dabai.launch.py             |
+| dabai d1                              | dabai_d1.launch.py          |
+| dabai dcw                             | dabai_dcw.launch.py         |
+| dabai dw                              | dabai_dw.launch.py          |
+| dabai pro                             | dabai_pro.launch.py         |
+| deeya                                 | deeya.launch.py             |
+| femto /femto w                        | femto.launch.py             |
+| femto mega                            | femto_mega.launch.py        |
+| femto bolt                            | femto_bolt.launch.py        |
+| gemini                                | gemini.launch.py            |
+| gemini                                | gemini.launch.py            |
+| gemini2 / dabai DCL                   | gemini2.launch.py           |
+| gemini2L                              | gemini2L.launch.py          |
+| gemini e                              | gemini_e.launch.py          |
+| gemini e lite                         | gemini_e_lite.launch.py     |
+| dabai max                             | dabai_max.launch.py         |
+| dabai max pro                         | dabai_max_pro.launch.py     |
+| gemini uw                             | gemini_uw.launch.py         |
+| dabai dcw2                            | dabai_dcw2.launch.py        |
+| dabai dw2                             | dabai_dw2.launch.py         |
+| gemini ew                             | gemini_ew.launch.py         |
+| gemini ew lite                        | gemini_ew_lite.launch.py    |
+| gemini 330 系列                       | gemini_330_series.launch.py |
 
 **所有启动文件本质上都是相似的，主要区别在于为同一系列中不同型号设置的参数的默认值。USB 标准的差异，例如 USB 2.0 与 USB 3.0，可能需要调整这些参数。如果遇到启动失败，请仔细查看规格手册。特别注意启动文件中的分辨率设置以及其他参数，以确保兼容性和最佳性能。**
 
 ## 产品支持
 
-| **产品列表** | **固件版本**               |
-| ------------ | -------------------------- |
-| Gemini 335   | 1.2.20                     |
-| Gemini 335L  | 1.2.20                     |
-| Femto Bolt   | 1.0.6/1.0.9                |
-| Femto Mega   | 1.1.7/1.2.7                |
-| Gemini 2 XL  | Obox：V1.2.5 VL：1.4.54   |
-| Astra 2      | 2.8.20                     |
-| Gemini 2 L   | 1.4.32                     |
-| Gemini 2     | 1.4.60 /1.4.76             |
-| Astra+       | 1.0.22/1.0.21/1.0.20/1.0.19 |
-| Femto        | 1.6.7                      |
-| Femto W      | 1.1.8                      |
-| DaBai        | 2436                       |
-| DaBai DCW    | 2460                       |
-| DaBai DW     | 2606                       |
-| Astra Mini Pro | 1007                     |
-| Gemini E     | 3460                       |
-| Gemini E Lite | 3606                      |
-| Gemini       | 3.0.18                     |
-| Astra Mini S Pro | 1.0.05                 |
+| **产品列表** | **固件版本**          |
+| ------------------ | --------------------------- |
+| Gemini 335         | 1.2.20                      |
+| Gemini 335L        | 1.2.20                      |
+| Femto Bolt         | 1.0.6/1.0.9                 |
+| Femto Mega         | 1.1.7/1.2.7                 |
+| Gemini 2 XL        | Obox：V1.2.5 VL：1.4.54     |
+| Astra 2            | 2.8.20                      |
+| Gemini 2 L         | 1.4.32                      |
+| Gemini 2           | 1.4.60 /1.4.76              |
+| Astra+             | 1.0.22/1.0.21/1.0.20/1.0.19 |
+| Femto              | 1.6.7                       |
+| Femto W            | 1.1.8                       |
+| DaBai              | 2436                        |
+| DaBai DCW          | 2460                        |
+| DaBai DW           | 2606                        |
+| Astra Mini Pro     | 1007                        |
+| Gemini E           | 3460                        |
+| Gemini E Lite      | 3606                        |
+| Gemini             | 3.0.18                      |
+| Astra Mini S Pro   | 1.0.05                      |
 
 ## DDS 调优
 
@@ -524,7 +543,6 @@ net.core.rmem_default=2147483647
 多个相机无图像
 
 - 可能是电源供应不足。为避免这种情况，请不要将所有相机连接到同一个集线器，并使用带电源的集线器。
-
 - 也可能是分辨率太高。要解决此问题，请尝试降低分辨率。
 
 为什么这里有那么多启动文件
@@ -537,4 +555,4 @@ net.core.rmem_default=2147483647
 
 根据 Apache 许可证 2.0 版（"许可证"）获得许可；除非遵守许可证，否则您不得使用此项目。您可以在以下网址获取许可证副本：
 
-[http://www.apache.org/licenses/LICENSE-2.0](http://www.apache.org/licenses/LICENSE-2.0)
+[http://www.apache.org/licenses/LICENSE-2.0](http://www.apache.org/licenses/LICENSE-2.0)

orbbec_camera/CMakeLists.txt

@@ -56,7 +56,7 @@ foreach (dep IN LISTS dependencies)
 endforeach ()
 
 find_package(PkgConfig REQUIRED)
-
+pkg_check_modules(FFMPEG REQUIRED libavcodec libavformat libavutil libswscale)
 
 if (USE_RK_HW_DECODER)
   pkg_search_module(RK_MPP REQUIRED rockchip_mpp)
@@ -110,12 +110,14 @@ set(COMMON_INCLUDE_DIRS
   $<INSTALL_INTERFACE:include>
   ${ORBBEC_INCLUDE_DIR}
   ${OpenCV_INCLUDED_DIRS}
+  ${FFMPEG_INCLUDE_DIRS}
   ${CMAKE_CURRENT_SOURCE_DIR}/tools
 )
 
 set(COMMON_LIBRARIES
   ${ORBBEC_SDK_LIBRARIES}
   ${OpenCV_LIBS}
+  ${FFMPEG_LIBRARIES}
   Eigen3::Eigen
   -lOrbbecSDK
   -L${ORBBEC_LIBS_DIR}
@@ -216,6 +218,7 @@ add_orbbec_executable(list_depth_work_mode_node tools/list_depth_work_mode.cpp)
 add_orbbec_executable(list_camera_profile_mode_node tools/list_camera_profile.cpp)
 
 add_orbbec_executable(topic_statistics_node tools/topic_statistics.cpp)
+add_orbbec_executable(mega_h26x_decode_node tools/mega_h26x_decode_node.cpp)
 
 
 add_library(frame_latency SHARED tools/frame_latency.cpp)
@@ -250,6 +253,7 @@ install(TARGETS list_devices_node
   list_depth_work_mode_node
   list_camera_profile_mode_node
   topic_statistics_node
+  mega_h26x_decode_node
   DESTINATION lib/${PROJECT_NAME}/)
 
 if (BUILD_TESTING)

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -393,6 +393,8 @@ class OBCameraNode {
   std::map<stream_index_pair, bool> flip_stream_;
   std::map<stream_index_pair, std::string> stream_name_;
   std::map<stream_index_pair, std::shared_ptr<image_publisher>> image_publishers_;
+  std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr>
+      camera_h26x_publishers_;
   std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>
       camera_info_publishers_;
 

orbbec_camera/src/ob_camera_node.cpp

@@ -388,8 +388,8 @@ void OBCameraNode::setupDevices() {
     RCLCPP_INFO_STREAM(logger_, "Setting color brightness to " << color_brightness_);
     TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_BRIGHTNESS_INT, color_brightness_);
   }
- // ir ae max
- if (device_->isPropertySupported(OB_PROP_IR_AE_MAX_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
+  // ir ae max
+  if (device_->isPropertySupported(OB_PROP_IR_AE_MAX_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
     RCLCPP_INFO_STREAM(logger_, "Setting IR AE max exposure to " << ir_ae_max_exposure_);
     TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_AE_MAX_EXPOSURE_INT, ir_ae_max_exposure_);
   }
@@ -546,14 +546,14 @@ void OBCameraNode::setupDepthPostProcessFilter() {
     } else if (filter_name == "NoiseRemovalFilter" && enable_noise_removal_filter_) {
       auto noise_removal_filter = filter->as<ob::NoiseRemovalFilter>();
       OBNoiseRemovalFilterParams params = noise_removal_filter->getFilterParams();
-      RCLCPP_INFO_STREAM(logger_, "Default noise removal filter params: "
-                                      << "disp_diff: " << params.disp_diff
-                                      << ", max_size: " << params.max_size);
+      RCLCPP_INFO_STREAM(
+          logger_, "Default noise removal filter params: " << "disp_diff: " << params.disp_diff
+                                                           << ", max_size: " << params.max_size);
       params.disp_diff = noise_removal_filter_min_diff_;
       params.max_size = noise_removal_filter_max_size_;
-      RCLCPP_INFO_STREAM(logger_, "Set noise removal filter params: "
-                                      << "disp_diff: " << params.disp_diff
-                                      << ", max_size: " << params.max_size);
+      RCLCPP_INFO_STREAM(logger_,
+                         "Set noise removal filter params: " << "disp_diff: " << params.disp_diff
+                                                             << ", max_size: " << params.max_size);
       if (noise_removal_filter_min_diff_ != -1 && noise_removal_filter_max_size_ != -1) {
         noise_removal_filter->setFilterParams(params);
       }
@@ -562,11 +562,11 @@ void OBCameraNode::setupDepthPostProcessFilter() {
           hdr_merge_gain_2_ != -1) {
         auto hdr_merge_filter = filter->as<ob::HdrMerge>();
         hdr_merge_filter->enable(true);
-        RCLCPP_INFO_STREAM(logger_, "Set HDR merge filter params: "
-                                        << "exposure_1: " << hdr_merge_exposure_1_
-                                        << ", gain_1: " << hdr_merge_gain_1_
-                                        << ", exposure_2: " << hdr_merge_exposure_2_
-                                        << ", gain_2: " << hdr_merge_gain_2_);
+        RCLCPP_INFO_STREAM(
+            logger_, "Set HDR merge filter params: " << "exposure_1: " << hdr_merge_exposure_1_
+                                                     << ", gain_1: " << hdr_merge_gain_1_
+                                                     << ", exposure_2: " << hdr_merge_exposure_2_
+                                                     << ", gain_2: " << hdr_merge_gain_2_);
         auto config = OBHdrConfig();
         config.enable = true;
         config.exposure_1 = hdr_merge_exposure_1_;
@@ -649,10 +649,10 @@ void OBCameraNode::setupProfiles() {
           throw std::runtime_error("Failed cast profile to VideoStreamProfile");
         }
         RCLCPP_DEBUG_STREAM(
-            logger_, "Sensor profile: "
-                         << "stream_type: " << magic_enum::enum_name(profile->type())
-                         << "Format: " << profile->format() << ", Width: " << profile->width()
-                         << ", Height: " << profile->height() << ", FPS: " << profile->fps());
+            logger_,
+            "Sensor profile: " << "stream_type: " << magic_enum::enum_name(profile->type())
+                               << "Format: " << profile->format() << ", Width: " << profile->width()
+                               << ", Height: " << profile->height() << ", FPS: " << profile->fps());
         supported_profiles_[elem].emplace_back(profile);
       }
       std::shared_ptr<ob::VideoStreamProfile> selected_profile;
@@ -1263,6 +1263,13 @@ void OBCameraNode::setupPublishers() {
     camera_info_publishers_[stream_index] = node_->create_publisher<CameraInfo>(
         topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
                            camera_info_qos_profile));
+
+    auto image_h264_qos_profile = getRMWQosProfileFromString(image_qos);
+    camera_h26x_publishers_[stream_index] =
+        node_->create_publisher<sensor_msgs::msg::CompressedImage>(
+            "/camera/color/h26x_encoded_data",
+            rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_h264_qos_profile),
+                        image_h264_qos_profile));
     if (isGemini335PID(pid)) {
       metadata_publishers_[stream_index] =
           node_->create_publisher<orbbec_camera_msgs::msg::Metadata>(
@@ -1383,7 +1390,7 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
     return;
   }
   std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);
-  if(!depth_frame_) {
+  if (!depth_frame_) {
     RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
     return;
   }
@@ -1802,6 +1809,7 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
   }
   CHECK_NOTNULL(image_publishers_[COLOR]);
   bool has_subscriber = image_publishers_[COLOR]->get_subscription_count() > 0;
+  has_subscriber = has_subscriber || camera_h26x_publishers_[COLOR]->get_subscription_count() > 0;
   if (enable_colored_point_cloud_ && depth_registration_cloud_pub_->get_subscription_count() > 0) {
     has_subscriber = true;
   }
@@ -1838,7 +1846,7 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
     }
   }
 #endif
-  if (!is_decoded) {
+  if (!is_decoded && !(frame->format() == OB_FORMAT_H264 || frame->format() == OB_FORMAT_H265)) {
     auto video_frame = softwareDecodeColorFrame(frame);
     if (!video_frame) {
       RCLCPP_ERROR_STREAM(logger_, "Failed to convert frame to video frame");
@@ -1897,6 +1905,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   has_subscriber =
       has_subscriber || (metadata_publishers_.count(stream_index) &&
                          metadata_publishers_[stream_index]->get_subscription_count() > 0);
+  has_subscriber =
+      has_subscriber || camera_h26x_publishers_[stream_index]->get_subscription_count() > 0;
   if (!has_subscriber) {
     return;
   }
@@ -1975,7 +1985,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     publishMetadata(frame, stream_index, camera_info.header);
   }
   CHECK_NOTNULL(image_publishers_[stream_index]);
-  if (image_publishers_[stream_index]->get_subscription_count() == 0) {
+  if (image_publishers_[stream_index]->get_subscription_count() == 0 &&
+      camera_h26x_publishers_[stream_index]->get_subscription_count() == 0) {
     return;
   }
   auto &image = images_[stream_index];
@@ -1995,18 +2006,32 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     auto depth_scale = video_frame->as<ob::DepthFrame>()->getValueScale();
     image = image * depth_scale;
   }
-  sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image());
+  if (frame->type() == OB_FRAME_COLOR &&
+      (frame->format() == OB_FORMAT_H264 || frame->format() == OB_FORMAT_H265)) {
+    sensor_msgs::msg::CompressedImage h264_image_msg;
+    h264_image_msg.header.stamp = timestamp;
+    if (frame->format() == OB_FORMAT_H264) {
+      h264_image_msg.format = "h264";
+    } else {
+      h264_image_msg.format = "h265";
+    }
+    h264_image_msg.data.resize(video_frame->dataSize());
+    memcpy(h264_image_msg.data.data(), video_frame->data(), video_frame->dataSize());
+    camera_h26x_publishers_[stream_index]->publish(std::move(h264_image_msg));
+  } else {
+    sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image());
 
-  cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], image)
-      .toImageMsg(*image_msg);
-  CHECK_NOTNULL(image_msg.get());
-  image_msg->header.stamp = timestamp;
-  image_msg->is_bigendian = false;
-  image_msg->step = width * unit_step_size_[stream_index];
-  image_msg->header.frame_id = frame_id;
-  CHECK(image_publishers_.count(stream_index) > 0);
-  saveImageToFile(stream_index, image, *image_msg);
-  image_publishers_[stream_index]->publish(std::move(image_msg));
+    cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], image)
+        .toImageMsg(*image_msg);
+    CHECK_NOTNULL(image_msg.get());
+    image_msg->header.stamp = timestamp;
+    image_msg->is_bigendian = false;
+    image_msg->step = width * unit_step_size_[stream_index];
+    image_msg->header.frame_id = frame_id;
+    CHECK(image_publishers_.count(stream_index) > 0);
+    saveImageToFile(stream_index, image, *image_msg);
+    image_publishers_[stream_index]->publish(std::move(image_msg));
+  }
   if (stream_index == COLOR && enable_color_undistortion_ &&
       color_undistortion_publisher_->get_subscription_count() > 0) {
     auto undistorted_image = undistortImage(image, intrinsic, distortion);

orbbec_camera/tools/mega_h26x_decode_node.cpp

@@ -0,0 +1,145 @@
+extern "C" {
+#include <libavcodec/avcodec.h>
+#include <libavformat/avformat.h>
+#include <libswscale/swscale.h>
+#include <libavutil/imgutils.h>
+#include <libavutil/time.h>
+}
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "orbbec_camera/ob_camera_node.h"
+#include <thread>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+
+#include "orbbec_camera/utils.h"
+#include <filesystem>
+#include <fstream>
+#include "diagnostic_msgs/msg/diagnostic_status.hpp"
+#include "libobsensor/hpp/Utils.hpp"
+
+class H264DecoderNode : public rclcpp::Node {
+ public:
+  H264DecoderNode() : Node("h264_decoder_node") {
+    avformat_network_init();
+    rclcpp::QoS qos_settings(30);
+    qos_settings.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
+    qos_settings.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
+    qos_settings.history(RMW_QOS_POLICY_HISTORY_KEEP_LAST);
+
+    compressed_image_subscriber_ = this->create_subscription<sensor_msgs::msg::CompressedImage>(
+        "/camera/color/h26x_encoded_data", qos_settings,
+        std::bind(&H264DecoderNode::compressedImageCallback, this, std::placeholders::_1));
+
+    rgb_image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>(
+        "/camera/color/h26x_decoder/image_raw", qos_settings);
+  }
+
+ private:
+  void decode_init(const sensor_msgs::msg::CompressedImage::SharedPtr msg) {
+    if (msg->format == "h264") {
+      codec_ = std::shared_ptr<const AVCodec>(avcodec_find_decoder(AV_CODEC_ID_H264),
+                                              [](const AVCodec*) {});
+      codec_context_ = std::shared_ptr<AVCodecContext>(avcodec_alloc_context3(codec_.get()),
+                                                       [](AVCodecContext* ctx) {
+                                                         if (ctx) {
+                                                           avcodec_free_context(&ctx);
+                                                         }
+                                                       });
+
+      if (avcodec_open2(codec_context_.get(), codec_.get(), nullptr) < 0) {
+        RCLCPP_ERROR(this->get_logger(), "Failed to open codec");
+        return;
+      }
+
+      frame_ = std::shared_ptr<AVFrame>(av_frame_alloc(), [](AVFrame* f) {
+        if (f) av_frame_free(&f);
+      });
+
+      packet_ = std::shared_ptr<AVPacket>(av_packet_alloc(), [](AVPacket* p) {
+        if (p) av_packet_free(&p);
+      });
+      codec_init_ = 0;
+    } else if (msg->format == "h265") {
+      codec_ = std::shared_ptr<const AVCodec>(avcodec_find_decoder(AV_CODEC_ID_HEVC),
+                                              [](const AVCodec*) {});
+      codec_context_ = std::shared_ptr<AVCodecContext>(avcodec_alloc_context3(codec_.get()),
+                                                       [](AVCodecContext* ctx) {
+                                                         if (ctx) {
+                                                           avcodec_free_context(&ctx);
+                                                         }
+                                                       });
+
+      if (avcodec_open2(codec_context_.get(), codec_.get(), nullptr) < 0) {
+        RCLCPP_ERROR(this->get_logger(), "Failed to open codec");
+        return;
+      }
+
+      frame_ = std::shared_ptr<AVFrame>(av_frame_alloc(), [](AVFrame* f) {
+        if (f) av_frame_free(&f);
+      });
+
+      packet_ = std::shared_ptr<AVPacket>(av_packet_alloc(), [](AVPacket* p) {
+        if (p) av_packet_free(&p);
+      });
+      codec_init_ = 0;
+    }
+  }
+  void decode_frame(const sensor_msgs::msg::CompressedImage::SharedPtr msg) {
+    av_packet_unref(packet_.get());
+    packet_->data = const_cast<uint8_t*>(msg->data.data());
+    packet_->size = msg->data.size();
+    std::stringstream ss;
+    const size_t bytes_to_print = std::min<size_t>(msg->data.size(), 32);
+    for (size_t i = 0; i < bytes_to_print; ++i) {
+      ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(msg->data[i]) << " ";
+    }
+    RCLCPP_INFO(this->get_logger(), "Data (hex): %s", ss.str().c_str());
+
+    send_ret_ = avcodec_send_packet(codec_context_.get(), packet_.get());
+    if (send_ret_ >= 0) {
+      receive_ret_ = avcodec_receive_frame(codec_context_.get(), frame_.get());
+      if (receive_ret_ >= 0) {
+        cv::Mat rgb_image(frame_->height, frame_->width, CV_8UC3);
+        SwsContext* sws_context = sws_getContext(frame_->width, frame_->height,
+                                                 static_cast<AVPixelFormat>(frame_->format),
+                                                 frame_->width, frame_->height, AV_PIX_FMT_BGR24,
+                                                 SWS_BILINEAR, nullptr, nullptr, nullptr);
+
+        uint8_t* dest[1] = {rgb_image.data};
+        int linesize[1] = {static_cast<int>(rgb_image.step1())};
+        sws_scale(sws_context, frame_->data, frame_->linesize, 0, frame_->height, dest, linesize);
+        sws_freeContext(sws_context);
+        auto rgb_image_msg =
+            cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb_image).toImageMsg();
+        rgb_image_msg->header.stamp = this->now();
+
+        rgb_image_publisher_->publish(*rgb_image_msg);
+      }
+    }
+  }
+  void compressedImageCallback(const sensor_msgs::msg::CompressedImage::SharedPtr msg) {
+    RCLCPP_INFO(this->get_logger(), "Format: %s", msg->format.c_str());
+    if (codec_init_) {
+      decode_init(msg);
+    }
+    decode_frame(msg);
+  }
+
+  rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr compressed_image_subscriber_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb_image_publisher_;
+  std::shared_ptr<const AVCodec> codec_;
+  std::shared_ptr<AVCodecContext> codec_context_;
+  std::shared_ptr<AVFrame> frame_;
+  std::shared_ptr<AVPacket> packet_;
+  int codec_init_ = 1;
+  int send_ret_;
+  int receive_ret_;
+};
+int main(int argc, char** argv) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<H264DecoderNode>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}