first work gst decoder

orbbec_camera/CMakeLists.txt

@@ -66,6 +66,9 @@ if (USE_GST_HW_DECODER)
     message(FATAL_ERROR "gstreamer-1.0 is not found")
   endif ()
   pkg_search_module(GST_APP REQUIRED gstreamer-app-1.0)
+  if (NOT GST_APP_FOUND)
+    message(FATAL_ERROR "gstreamer-app-1.0 is not found")
+  endif ()
 endif ()
 execute_process(COMMAND uname -m OUTPUT_VARIABLE MACHINES)
 execute_process(COMMAND getconf LONG_BIT OUTPUT_VARIABLE MACHINES_BIT)

orbbec_camera/include/orbbec_camera/gst_decoder.h

@@ -1,13 +1,7 @@
 #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);
 
@@ -24,6 +18,9 @@ class GstreamerMjpegDecoder : public MjpegDecoder {
   GstBufferPool* buffer_pool_ = nullptr;
   GstElement* pipeline_ = nullptr;
   GstElement* appsrc_ = nullptr;
+  GstElement* jpegparse_ = nullptr;
+  GstElement* jpegdec_ = nullptr;
+  GstElement* videoconvert_ = nullptr;
   GstElement* appsink_ = nullptr;
 };
 

orbbec_camera/include/orbbec_camera/mjpeg_decoder.h

@@ -5,6 +5,13 @@
 #include "libobsensor/ObSensor.hpp"
 
 namespace orbbec_camera {
+enum HWDecoder {
+  ROCKCHIP_MPP = 0,
+  NV_JPEG_DEC = 1,
+  AMLOGIC_CODEC = 2,
+  AV_CODEC = 3,
+};
+
 class MjpegDecoder {
  public:
   MjpegDecoder(int width, int height);

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -57,6 +57,11 @@
 #include "orbbec_camera/d2c_viewer.h"
 #include "magic_enum/magic_enum.hpp"
 #include "mjpeg_decoder.h"
+#if defined(USE_RK_HW_DECODER)
+#include "orbbec_camera/rk_mpp_decoder.h"
+#elif defined(USE_GST_HW_DECODER)
+#include "orbbec_camera/gst_decoder.h"
+#endif
 
 #define STREAM_NAME(sip)                                                                       \
   (static_cast<std::ostringstream&&>(std::ostringstream()                                      \
@@ -404,5 +409,6 @@ class OBCameraNode {
   // mjpeg decoder
   std::shared_ptr<MjpegDecoder> mjpeg_decoder_ = nullptr;
   uint8_t* rgb_buffer_ = nullptr;
+  HWDecoder hw_decoder_ = HWDecoder::AV_CODEC;
 };
 }  // namespace orbbec_camera

orbbec_camera/launch/gemini2.launch.py

@@ -67,6 +67,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('hw_decoder', default_value='3'),
     ]
 
     # Node configuration

orbbec_camera/src/gst_decoder.cpp

@@ -2,6 +2,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <gst/app/gstappsrc.h>
 #include <gst/app/gstappsink.h>
+#include <glog/logging.h>
 
 namespace orbbec_camera {
 std::string hwDecoderToString(HWDecoder hw_decoder) {
@@ -12,6 +13,8 @@ std::string hwDecoderToString(HWDecoder hw_decoder) {
       return "nvjpegdec";
     case HWDecoder::AMLOGIC_CODEC:
       return "amlvenc";
+    case HWDecoder::AV_CODEC:
+      return "avdec_mjpeg";
     default:
       return "unknown";
   }
@@ -21,8 +24,11 @@ GstreamerMjpegDecoder::GstreamerMjpegDecoder(int width, int height, HWDecoder hw
     : MjpegDecoder(width, height),
       hw_decoder_(hwDecoderToString(hw_decoder)),
       buffer_size_(width * height * 3) {
+  gst_init(NULL, NULL);
   buffer_pool_ = gst_buffer_pool_new();
+  CHECK_NOTNULL(buffer_pool_);
   GstStructure* config = gst_buffer_pool_get_config(buffer_pool_);
+  CHECK_NOTNULL(config);
   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");
@@ -38,15 +44,47 @@ GstreamerMjpegDecoder::GstreamerMjpegDecoder(int width, int height, HWDecoder hw
                         "gst buffer pool set active error");
     throw std::runtime_error("gst buffer pool set active error");
   }
-  std::string pipeline_str = "appsrc name=appsrc0 is-live=true do-timestamp=true ! " + hw_decoder_ +
+  // Create GStreamer elements
-                             " ! videoconvert ! video/x-raw,format=RGB ! appsink name=appsink0";
+  appsrc_ = gst_element_factory_make("appsrc", "appsrc");
-  pipeline_ = gst_parse_launch(pipeline_str.c_str(), NULL);
+  jpegparse_ = gst_element_factory_make("jpegparse", "jpegparse");
+  jpegdec_ = gst_element_factory_make(hw_decoder_.c_str(), hw_decoder_.c_str());
+  videoconvert_ = gst_element_factory_make("videoconvert", "videoconvert");
+  appsink_ = gst_element_factory_make("appsink", "appsink");
+
+  // Check for null pointers
+  if (!appsrc_ || !jpegparse_ || !jpegdec_ || !videoconvert_ || !appsink_) {
+    throw std::runtime_error("Failed to create GStreamer elements");
+  }
+
+  // Set element properties
+  g_object_set(G_OBJECT(appsrc_), "caps",
+               gst_caps_new_simple("image/jpeg", "width", G_TYPE_INT, width, "height", G_TYPE_INT,
+                                   height, "framerate", GST_TYPE_FRACTION, 0, 1, NULL),
+               NULL);
+
+  g_object_set(G_OBJECT(appsink_), "blocksize", buffer_size_, "sync", FALSE, "max-buffers", 1,
+               "drop", TRUE, NULL);
+
+  g_object_set(G_OBJECT(appsink_), "caps",
+               gst_caps_new_simple("video/x-raw", "format", G_TYPE_STRING, "RGB", "width",
+                                   G_TYPE_INT, width, "height", G_TYPE_INT, height, NULL),
+               NULL);
+
+  // Create pipeline and add elements
+  pipeline_ = gst_pipeline_new("pipeline");
   if (!pipeline_) {
-    RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), "gst parse launch error");
+    throw std::runtime_error("Failed to create pipeline");
-    throw std::runtime_error("gst parse launch error");
+  }
+
+  gst_bin_add_many(GST_BIN(pipeline_), appsrc_, jpegparse_, jpegdec_, videoconvert_, appsink_,
+                   NULL);
+  if (!gst_element_link_many(appsrc_, jpegparse_, jpegdec_, videoconvert_, appsink_, NULL)) {
+    throw std::runtime_error("Failed to link GStreamer elements");
+  }
+  // Set pipeline to playing state
+  if (gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) {
+    throw std::runtime_error("Failed to set GStreamer pipeline to playing state");
   }
-  appsrc_ = gst_bin_get_by_name(GST_BIN(pipeline_), "appsrc0");
-  appsink_ = gst_bin_get_by_name(GST_BIN(pipeline_), "appsink0");
 }
 
 GstreamerMjpegDecoder::~GstreamerMjpegDecoder() {
@@ -59,37 +97,64 @@ GstreamerMjpegDecoder::~GstreamerMjpegDecoder() {
     gst_buffer_pool_set_active(buffer_pool_, FALSE);
     g_object_unref(buffer_pool_);
   }
+  gst_deinit();
 }
 
 bool GstreamerMjpegDecoder::decode(const std::shared_ptr<ob::ColorFrame>& frame, uint8_t* dest) {
   GstBuffer* buffer = NULL;
   GstMapInfo map;
-
+  // Acquire buffer from pool
   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;
   }
+  // Map buffer and copy frame data
+  if (gst_buffer_map(buffer, &map, GST_MAP_WRITE)) {
+    if (frame->dataSize() <= map.size) {
+      memcpy(map.data, frame->data(), frame->dataSize());
+    } else {
+      RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"),
+                          "Frame data size exceeds buffer size");
+      gst_buffer_unmap(buffer, &map);
+      return false;
+    }
+    gst_buffer_unmap(buffer, &map);
+  } else {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), "Failed to map buffer");
+    return false;
+  }
 
-  gst_buffer_map(buffer, &map, GST_MAP_WRITE);
+  // Push buffer to appsrc
-  memcpy(map.data, frame->data(), frame->dataSize());
+  GstFlowReturn flow_return = gst_app_src_push_buffer(GST_APP_SRC(appsrc_), buffer);
-  gst_buffer_unmap(buffer, &map);  // Unmap the buffer after copying
+  if (flow_return != GST_FLOW_OK) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"),
+                        "Failed to push buffer to appsrc, GstFlowReturn: " << flow_return);
+    return false;
+  }
+
+  // Pull sample from appsink
+  GstClockTime timeout = GST_SECOND;
+  GstSample* sample = gst_app_sink_try_pull_sample(GST_APP_SINK(appsink_), timeout);
 
-  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);
+    if (gst_buffer_map(outBuffer, &mapInfo, GST_MAP_READ)) {
-
+      // Copy data to destination buffer
-    // Copy data to destination buffer
+      memcpy(dest, mapInfo.data, mapInfo.size);
-    memcpy(dest, mapInfo.data, mapInfo.size);
+      gst_buffer_unmap(outBuffer, &mapInfo);
-
+    } else {
-    gst_buffer_unmap(outBuffer, &mapInfo);
+      RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"),
+                          "Failed to map output buffer");
+      gst_sample_unref(sample);
+      return false;
+    }
     gst_sample_unref(sample);
     return true;
   } else {
-    RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), "Failed to decode frame");
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"),
+                        "Failed to decode frame " << strerror(errno) << " " << errno);
     return false;
   }
 }

orbbec_camera/src/ob_camera_node.cpp

@@ -20,6 +20,8 @@
 
 #if defined(USE_RK_HW_DECODER)
 #include "orbbec_camera/rk_mpp_decoder.h"
+#elif defined(USE_GST_HW_DECODER)
+#include "orbbec_camera/gst_decoder.h"
 #endif
 
 namespace orbbec_camera {
@@ -45,16 +47,18 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
   compression_params_.push_back(cv::IMWRITE_PNG_STRATEGY_DEFAULT);
   setupDefaultImageFormat();
   setupTopics();
+#if defined(USE_RK_HW_DECODER)
+  mjpeg_decoder_ = std::make_unique<RKMjpegDecoder>(width_[COLOR], height_[COLOR]);
+#elif defined(USE_GST_HW_DECODER)
+  mjpeg_decoder_ =
+      std::make_unique<GstreamerMjpegDecoder>(width_[COLOR], height_[COLOR], hw_decoder_);
+#endif
   startStreams();
   if (enable_d2c_viewer_) {
     auto rgb_qos = getRMWQosProfileFromString(image_qos_[COLOR]);
     auto depth_qos = getRMWQosProfileFromString(image_qos_[DEPTH]);
     d2c_viewer_ = std::make_unique<D2CViewer>(node_, rgb_qos, depth_qos);
   }
-
-#if defined(USE_RK_HW_DECODER)
-  mjpeg_decoder_ = std::make_unique<RKMjpegDecoder>(width_[COLOR], height_[COLOR]);
-#endif
 }
 
 template <class T>
@@ -424,6 +428,9 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<int>(soft_filter_speckle_size_, "soft_filter_speckle_size", -1);
   setAndGetNodeParameter<double>(liner_accel_cov_, "linear_accel_cov", 0.0003);
   setAndGetNodeParameter<double>(angular_vel_cov_, "angular_vel_cov", 0.02);
+  int hw_decoder = 0;
+  setAndGetNodeParameter<int>(hw_decoder, "hw_decoder", 3);
+  hw_decoder_ = static_cast<HWDecoder>(hw_decoder);
 }
 
 void OBCameraNode::setupTopics() {
@@ -741,7 +748,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   auto frame_format = frame->format();
   if (frame->type() == OB_FRAME_COLOR && frame_format != OB_FORMAT_RGB888) {
     if (frame_format == OB_FORMAT_MJPG || frame_format == OB_FORMAT_MJPEG) {
-#if defined(USE_RK_HW_DECODER)
+#if defined(USE_RK_HW_DECODER) || defined(USE_GST_HW_DECODER)
       CHECK_NOTNULL(mjpeg_decoder_.get());
       video_frame = frame->as<ob::ColorFrame>();
       const auto &color_frame = frame->as<ob::ColorFrame>();