add gst decoder

orbbec_camera/CMakeLists.txt

@@ -9,6 +9,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -O3")
 set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fPIC -g")
 set(CMAKE_BUILD_TYPE "Release")
 option(USE_RK_HW_DECODER "Use Rockchip hardware decoder" OFF)
+option(USE_GST_HW_DECODER "Use Gstreamer hardware decoder" ON)
 
 if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Werror)
@@ -58,6 +59,14 @@ if (USE_RK_HW_DECODER)
   pkg_search_module(RGA REQUIRED librga)
 endif ()
 
+if (USE_GST_HW_DECODER)
+  find_package(PkgConfig REQUIRED)
+  pkg_search_module(GST REQUIRED gstreamer-1.0)
+  if (NOT GST_FOUND)
+    message(FATAL_ERROR "gstreamer-1.0 is not found")
+  endif ()
+  pkg_search_module(GST_APP REQUIRED gstreamer-app-1.0)
+endif ()
 execute_process(COMMAND uname -m OUTPUT_VARIABLE MACHINES)
 execute_process(COMMAND getconf LONG_BIT OUTPUT_VARIABLE MACHINES_BIT)
 message(STATUS "ORRBEC Machine : ${MACHINES}")
@@ -83,6 +92,8 @@ set(common_include_dirs
   ${GLOG_INCLUDED_DIRS}
   ${RK_MPP_INCLUDE_DIRS}
   ${RGA_INCLUDE_DIRS}
+  ${GST_INCLUDE_DIRS}
+  ${GST_APP_INCLUDE_DIRS}
 )
 
 set(common_libraries
@@ -112,6 +123,11 @@ if (USE_RK_HW_DECODER)
   list(APPEND source_files src/rk_mpp_decoder.cpp)
 endif ()
 
+if (USE_GST_HW_DECODER)
+  add_definitions(-DUSE_GST_HW_DECODER)
+  list(APPEND source_files src/gst_decoder.cpp)
+endif ()
+
 macro(add_orbbec_executable target source)
   add_executable(${target} ${source})
   target_include_directories(${target} PUBLIC ${common_include_dirs})
@@ -121,6 +137,11 @@ macro(add_orbbec_executable target source)
     target_link_libraries(${target}
       ${RGA_LIBRARIES}
     )
+  elseif (USE_GST_HW_DECODER)
+    target_link_libraries(${target}
+      ${GST_LIBRARIES}
+      ${GST_APP_LIBRARIES}
+    )
   endif ()
 endmacro()
 
@@ -136,6 +157,11 @@ if (USE_RK_HW_DECODER)
   target_link_libraries(${PROJECT_NAME}
     ${RGA_LIBRARIES}
   )
+elseif (USE_GST_HW_DECODER)
+  target_link_libraries(${PROJECT_NAME}
+    ${GST_LIBRARIES}
+    ${GST_APP_LIBRARIES}
+  )
 endif ()
 
 rclcpp_components_register_node(${PROJECT_NAME}

orbbec_camera/include/orbbec_camera/gst_decoder.h

@@ -0,0 +1,30 @@
+#pragma once
+#include "mjpeg_decoder.h"
+#include <gst/gst.h>
+
+namespace orbbec_camera {
+enum class HWDecoder : int {
+  ROCKCHIP_MPP = 0,
+  NV_JPEG_DEC = 1,
+  AMLOGIC_CODEC = 2,
+};
+
+std::string hwDecoderToString(HWDecoder hw_decoder);
+
+class GstreamerMjpegDecoder : public MjpegDecoder {
+ public:
+  GstreamerMjpegDecoder(int width, int height, HWDecoder hw_decoder);
+  ~GstreamerMjpegDecoder() override;
+
+  bool decode(const std::shared_ptr<ob::ColorFrame>& frame, uint8_t* dest) override;
+
+ private:
+  std::string hw_decoder_;
+  guint buffer_size_ = 0;
+  GstBufferPool* buffer_pool_ = nullptr;
+  GstElement* pipeline_ = nullptr;
+  GstElement* appsrc_ = nullptr;
+  GstElement* appsink_ = nullptr;
+};
+
+}  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/mjpeg_decoder.h

@@ -18,7 +18,6 @@ class MjpegDecoder {
  protected:
   int width_ = 0;
   int height_ = 0;
-  uint8_t *rgb_buffer_ = nullptr;
   std::string error_msg_;
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/rk_mpp_decoder.h

@@ -36,6 +36,7 @@ class RKMjpegDecoder : public MjpegDecoder {
   MppBufferGroup mpp_packet_group_ = nullptr;
   MppTask mpp_task_ = nullptr;
   uint32_t need_split_ = 0;
+  uint8_t * rgb_buffer_ = nullptr;
 };
 
 }  // namespace orbbec_camera

orbbec_camera/src/gst_decoder.cpp

@@ -0,0 +1,97 @@
+#include "orbbec_camera/gst_decoder.h"
+#include <rclcpp/rclcpp.hpp>
+#include <gst/app/gstappsrc.h>
+#include <gst/app/gstappsink.h>
+
+namespace orbbec_camera {
+std::string hwDecoderToString(HWDecoder hw_decoder) {
+  switch (hw_decoder) {
+    case HWDecoder::ROCKCHIP_MPP:
+      return "mppjpegdec";
+    case HWDecoder::NV_JPEG_DEC:
+      return "nvjpegdec";
+    case HWDecoder::AMLOGIC_CODEC:
+      return "amlvenc";
+    default:
+      return "unknown";
+  }
+}
+
+GstreamerMjpegDecoder::GstreamerMjpegDecoder(int width, int height, HWDecoder hw_decoder)
+    : MjpegDecoder(width, height),
+      hw_decoder_(hwDecoderToString(hw_decoder)),
+      buffer_size_(width * height * 3) {
+  buffer_pool_ = gst_buffer_pool_new();
+  GstStructure* config = gst_buffer_pool_get_config(buffer_pool_);
+  gst_buffer_pool_config_set_params(config, NULL, buffer_size_, 0, 0);
+  if (hw_decoder_ == "unknown") {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), "hw decoder is unknown");
+    throw std::runtime_error("hw decoder is unknown");
+  }
+  if (!gst_buffer_pool_set_config(buffer_pool_, config)) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"),
+                        "gst buffer pool set config error");
+    throw std::runtime_error("gst buffer pool set config error");
+  }
+  if (gst_buffer_pool_set_active(buffer_pool_, TRUE) != TRUE) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"),
+                        "gst buffer pool set active error");
+    throw std::runtime_error("gst buffer pool set active error");
+  }
+  std::string pipeline_str = "appsrc name=src is-live=true do-timestamp=true ! " + hw_decoder_ +
+                             " ! videoconvert ! video/x-raw,format=RGB ! appsink name=sink";
+  pipeline_ = gst_parse_launch(pipeline_str.c_str(), NULL);
+  if (!pipeline_) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), "gst parse launch error");
+    throw std::runtime_error("gst parse launch error");
+  }
+  appsrc_ = gst_bin_get_by_name(GST_BIN(pipeline_), "appsrc0");
+  appsink_ = gst_bin_get_by_name(GST_BIN(pipeline_), "appsink0");
+}
+
+GstreamerMjpegDecoder::~GstreamerMjpegDecoder() {
+  if (pipeline_) {
+    gst_element_set_state(pipeline_, GST_STATE_NULL);
+    gst_object_unref(pipeline_);
+    pipeline_ = nullptr;
+  }
+  if (buffer_pool_) {
+    gst_buffer_pool_set_active(buffer_pool_, FALSE);
+    g_object_unref(buffer_pool_);
+  }
+}
+
+bool GstreamerMjpegDecoder::decode(const std::shared_ptr<ob::ColorFrame>& frame, uint8_t* dest) {
+  GstBuffer* buffer = NULL;
+  GstMapInfo map;
+
+  if (gst_buffer_pool_acquire_buffer(buffer_pool_, &buffer, NULL) != GST_FLOW_OK) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"),
+                        "gst buffer pool acquire buffer error");
+    return false;
+  }
+
+  gst_buffer_map(buffer, &map, GST_MAP_WRITE);
+  memcpy(map.data, frame->data(), frame->dataSize());
+  gst_buffer_unmap(buffer, &map);  // Unmap the buffer after copying
+
+  gst_app_src_push_buffer(GST_APP_SRC(appsrc_), buffer);
+  GstSample* sample = gst_app_sink_pull_sample(GST_APP_SINK(appsink_));
+  if (sample) {
+    GstBuffer* outBuffer = gst_sample_get_buffer(sample);
+    GstMapInfo mapInfo;
+    gst_buffer_map(outBuffer, &mapInfo, GST_MAP_READ);
+
+    // Copy data to destination buffer
+    memcpy(dest, mapInfo.data, mapInfo.size);
+
+    gst_buffer_unmap(outBuffer, &mapInfo);
+    gst_sample_unref(sample);
+    return true;
+  } else {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), "Failed to decode frame");
+    return false;
+  }
+}
+
+}  // namespace orbbec_camera

orbbec_camera/src/mjpeg_decoder.cpp

@@ -2,14 +2,7 @@
 #include <orbbec_camera/mjpeg_decoder.h>
 
 namespace orbbec_camera {
-MjpegDecoder::MjpegDecoder(int width, int height) : width_(width), height_(height) {
-  rgb_buffer_ = new uint8_t[width_ * height_ * 3];
-}
-MjpegDecoder::~MjpegDecoder() {
-  if (rgb_buffer_) {
-    delete[] rgb_buffer_;
-    rgb_buffer_ = nullptr;
-  }
-}
+MjpegDecoder::MjpegDecoder(int width, int height) : width_(width), height_(height) {}
+MjpegDecoder::~MjpegDecoder() {}
 
 }  // namespace orbbec_camera

orbbec_camera/src/rk_mpp_decoder.cpp

@@ -5,6 +5,7 @@
 namespace orbbec_camera {
 
 RKMjpegDecoder::RKMjpegDecoder(int width, int height) : MjpegDecoder(width, height) {
+  rgb_buffer_ = new uint8_t[width_ * height_ * 3];
   MPP_RET ret = mpp_create(&mpp_ctx_, &mpp_api_);
   if (ret != MPP_OK) {
     RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"), "mpp_create failed, ret = " << ret);
@@ -100,6 +101,9 @@ RKMjpegDecoder::~RKMjpegDecoder() {
     mpp_destroy(mpp_ctx_);
     mpp_ctx_ = nullptr;
   }
+  if(rgb_buffer_){
+    delete[] rgb_buffer_;
+  }
 }
 
 bool RKMjpegDecoder::mppFrame2RGB(const MppFrame frame, uint8_t *data) {