add mpp hardware decode mjpeg

orbbec_camera/CMakeLists.txt

@@ -8,9 +8,10 @@ set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG}  -fPIC -g")
 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)
 
 if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+  add_compile_options(-Wall -Wextra -Werror)
 endif ()
 
 # find dependencies
@@ -49,6 +50,14 @@ if (NOT GLOG_FOUND)
   message(FATAL_ERROR "glog is not found")
 endif ()
 
+if (USE_RK_HW_DECODER)
+  pkg_search_module(RK_MPP REQUIRED rockchip_mpp)
+  if (NOT RK_MPP_FOUND)
+    message(FATAL_ERROR "rockchip_mpp is not found")
+  endif ()
+  pkg_search_module(RGA REQUIRED librga)
+endif ()
+
 execute_process(COMMAND uname -m OUTPUT_VARIABLE MACHINES)
 execute_process(COMMAND getconf LONG_BIT OUTPUT_VARIABLE MACHINES_BIT)
 message(STATUS "ORRBEC Machine : ${MACHINES}")
@@ -72,6 +81,8 @@ set(common_include_dirs
   ${ORBBEC_INCLUDE_DIR}
   ${OpenCV_INCLUDED_DIRS}
   ${GLOG_INCLUDED_DIRS}
+  ${RK_MPP_INCLUDE_DIRS}
+  ${RGA_INCLUDE_DIRS}
 )
 
 set(common_libraries
@@ -81,17 +92,10 @@ set(common_libraries
   ${GLOG_LIBRARIES}
   -lOrbbecSDK
   -L${ORBBEC_LIBS}
+  ${RK_MPP_LIBRARIES}
 )
 
-macro(add_orbbec_executable target source)
+set(source_files
-  add_executable(${target} ${source})
-  target_include_directories(${target} PUBLIC ${common_include_dirs})
-  target_link_libraries(${target} ${common_libraries} ${PROJECT_NAME})
-  ament_target_dependencies(${target} ${dependencies})
-endmacro()
-
-# Define library and nodes
-add_library(${PROJECT_NAME} SHARED
   src/d2c_viewer.cpp
   src/dynamic_params.cpp
   src/ob_camera_node_driver.cpp
@@ -100,17 +104,44 @@ add_library(${PROJECT_NAME} SHARED
   src/ros_service.cpp
   src/synced_imu_publisher.cpp
   src/utils.cpp
+  src/mjpeg_decoder.cpp
+)
+
+if (USE_RK_HW_DECODER)
+  add_definitions(-DUSE_RK_HW_DECODER)
+  list(APPEND source_files src/rk_mpp_decoder.cpp)
+endif ()
+
+macro(add_orbbec_executable target source)
+  add_executable(${target} ${source})
+  target_include_directories(${target} PUBLIC ${common_include_dirs})
+  target_link_libraries(${target} ${common_libraries} ${PROJECT_NAME})
+  ament_target_dependencies(${target} ${dependencies})
+  if (USE_RK_HW_DECODER)
+    target_link_libraries(${target}
+      ${RGA_LIBRARIES}
+    )
+  endif ()
+endmacro()
+
+# Define library and nodes
+add_library(${PROJECT_NAME} SHARED
+  ${source_files}
 )
 
 ament_target_dependencies(${PROJECT_NAME} ${dependencies})
 target_include_directories(${PROJECT_NAME} PUBLIC ${common_include_dirs})
 target_link_libraries(${PROJECT_NAME} ${common_libraries})
+if (USE_RK_HW_DECODER)
+  target_link_libraries(${PROJECT_NAME}
+    ${RGA_LIBRARIES}
+  )
+endif ()
 
 rclcpp_components_register_node(${PROJECT_NAME}
   PLUGIN "orbbec_camera::OBCameraNodeDriver"
   EXECUTABLE orbbec_camera_node
 )
-
 # Add nodes using the macro
 add_orbbec_executable(list_devices_node src/list_devices_node.cpp)
 add_orbbec_executable(ob_cleanup_shm_node src/ob_cleanup_shm.cpp)

orbbec_camera/include/orbbec_camera/mjpeg_decoder.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <string>
+#include <vector>
+#include "libobsensor/ObSensor.hpp"
+
+namespace orbbec_camera {
+class MjpegDecoder {
+ public:
+  MjpegDecoder(int width, int height);
+
+  virtual ~MjpegDecoder();
+
+  virtual bool decode(const std::shared_ptr<ob::ColorFrame> &frame, uint8_t *dest) = 0;
+
+  std::string getErrorMsg() const { return error_msg_; }
+
+ protected:
+  int width_ = 0;
+  int height_ = 0;
+  uint8_t *rgb_buffer_ = nullptr;
+  std::string error_msg_;
+};
+}  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -56,6 +56,7 @@
 #include "orbbec_camera/dynamic_params.h"
 #include "orbbec_camera/d2c_viewer.h"
 #include "magic_enum/magic_enum.hpp"
+#include "mjpeg_decoder.h"
 
 #define STREAM_NAME(sip)                                                                       \
   (static_cast<std::ostringstream&&>(std::ostringstream()                                      \
@@ -156,8 +157,8 @@ class OBCameraNode {
 
   void setupPublishers();
 
-  void publishStaticTF(const rclcpp::Time& t, const tf2::Vector3& trans,
+  void publishStaticTF(const rclcpp::Time& t, const tf2::Vector3& trans, const tf2::Quaternion& q,
-                       const tf2::Quaternion& q, const std::string& from, const std::string& to);
+                       const std::string& from, const std::string& to);
 
   void calcAndPublishStaticTransform();
 
@@ -254,6 +255,8 @@ class OBCameraNode {
 
   void onNewFrameSetCallback(const std::shared_ptr<ob::FrameSet>& frame_set);
 
+  std::shared_ptr<ob::Frame> softwareDecodeColorFrame(const std::shared_ptr<ob::Frame>& frame);
+
   void onNewFrameCallback(const std::shared_ptr<ob::Frame>& frame,
                           const stream_index_pair& stream_index);
 
@@ -398,5 +401,8 @@ class OBCameraNode {
   double angular_vel_cov_ = 0.0001;
   std::deque<IMUData> imu_history_;
   IMUData accel_data_{ACCEL, {0, 0, 0}, -1.0};
+  // mjpeg decoder
+  std::shared_ptr<MjpegDecoder> mjpeg_decoder_ = nullptr;
+  uint8_t* rgb_buffer_ = nullptr;
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/rk_mpp_decoder.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include "mjpeg_decoder.h"
+#include <rga/RgaApi.h>
+#include <rockchip/mpp_buffer.h>
+#include <rockchip/mpp_err.h>
+#include <rockchip/mpp_frame.h>
+#include <rockchip/mpp_log.h>
+#include <rockchip/mpp_packet.h>
+#include <rockchip/mpp_rc_defs.h>
+#include <rockchip/mpp_task.h>
+#include <rockchip/rk_mpi.h>
+#define MPP_ALIGN(x, a) (((x) + (a)-1) & ~((a)-1))
+
+namespace orbbec_camera {
+class RKMjpegDecoder : public MjpegDecoder {
+ public:
+  RKMjpegDecoder(int width, int height);
+
+  ~RKMjpegDecoder() override;
+
+  bool decode(const std::shared_ptr<ob::ColorFrame>& frame, uint8_t* dest) override;
+
+  bool mppFrame2RGB(const MppFrame frame, uint8_t* data);
+
+ private:
+  MppCtx mpp_ctx_ = nullptr;
+  MppApi* mpp_api_ = nullptr;
+  MppPacket mpp_packet_ = nullptr;
+  MppFrame mpp_frame_ = nullptr;
+  MppDecCfg mpp_dec_cfg_ = nullptr;
+  MppBuffer mpp_frame_buffer_ = nullptr;
+  MppBuffer mpp_packet_buffer_ = nullptr;
+  uint8_t* data_buffer_ = nullptr;
+  MppBufferGroup mpp_frame_group_ = nullptr;
+  MppBufferGroup mpp_packet_group_ = nullptr;
+  MppTask mpp_task_ = nullptr;
+  uint32_t need_split_ = 0;
+};
+
+}  // namespace orbbec_camera

orbbec_camera/src/mjpeg_decoder.cpp

@@ -0,0 +1,15 @@
+
+#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;
+  }
+}
+
+}  // namespace orbbec_camera

orbbec_camera/src/ob_camera_node.cpp

@@ -17,6 +17,11 @@
 
 #include "orbbec_camera/utils.h"
 #include <filesystem>
+
+#if defined(USE_RK_HW_DECODER)
+#include "orbbec_camera/rk_mpp_decoder.h"
+#endif
+
 namespace orbbec_camera {
 using namespace std::chrono_literals;
 
@@ -46,6 +51,10 @@ OBCameraNode::OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> devic
     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>
@@ -697,24 +706,58 @@ void OBCameraNode::onNewFrameSetCallback(const std::shared_ptr<ob::FrameSet>& fr
   }
 }
 
+std::shared_ptr<ob::Frame> OBCameraNode::softwareDecodeColorFrame(
+    const std::shared_ptr<ob::Frame>& frame) {
+  if (frame == nullptr) {
+    return nullptr;
+  }
+  if (frame->format() == OB_FORMAT_RGB888) {
+    return frame;
+  }
+  if (!setupFormatConvertType(frame->format())) {
+    RCLCPP_ERROR(logger_, "Unsupported color format: %d", frame->format());
+    return nullptr;
+  }
+  auto color_frame = format_convert_filter_.process(frame);
+  if (color_frame == nullptr) {
+    RCLCPP_ERROR_SKIPFIRST_THROTTLE(logger_, *(node_->get_clock()), 1000,
+                                    "Failed to convert frame to RGB format");
+    return nullptr;
+  }
+  return color_frame;
+}
+
 void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame>& frame,
                                       const stream_index_pair& stream_index) {
   if (frame == nullptr) {
     return;
   }
   std::shared_ptr<ob::VideoFrame> video_frame;
-  if (frame->type() == OB_FRAME_COLOR && frame->format() != OB_FORMAT_RGB888) {
+  bool hw_decode = false;
-    if (!setupFormatConvertType(frame->format())) {
+  auto frame_format = frame->format();
-      RCLCPP_ERROR(logger_, "Unsupported color format: %d", frame->format());
+  if (frame->type() == OB_FRAME_COLOR && frame_format != OB_FORMAT_RGB888) {
-      return;
+    if (frame_format == OB_FORMAT_MJPG || frame_format == OB_FORMAT_MJPEG) {
+#if defined(USE_RK_HW_DECODER)
+      CHECK_NOTNULL(mjpeg_decoder_.get());
+      video_frame = frame->as<ob::ColorFrame>();
+      const auto& color_frame = frame->as<ob::ColorFrame>();
+      if (rgb_buffer_ == nullptr) {
+        rgb_buffer_ = new uint8_t[video_frame->width() * video_frame->height() * 3];
+      }
+      bool ret = mjpeg_decoder_->decode(color_frame, rgb_buffer_);
+      if (!ret) {
+        RCLCPP_ERROR(logger_,"Decode frame failed");
+        return;
+      }
+      hw_decode = true;
+#else
+      auto covert_frame = softwareDecodeColorFrame(frame);
+      video_frame = covert_frame->as<ob::ColorFrame>();
+#endif
+    } else {
+      auto covert_frame = softwareDecodeColorFrame(frame);
+      video_frame = covert_frame->as<ob::ColorFrame>();
     }
-    auto color_frame = format_convert_filter_.process(frame);
-    if (color_frame == nullptr) {
-      RCLCPP_ERROR_SKIPFIRST_THROTTLE(logger_, *(node_->get_clock()), 1000,
-                                      "Failed to convert frame to RGB format");
-      return;
-    }
-    video_frame = color_frame->as<ob::ColorFrame>();
   } else if (frame->type() == OB_FRAME_COLOR) {
     video_frame = frame->as<ob::ColorFrame>();
   } else if (frame->type() == OB_FRAME_DEPTH) {
@@ -735,7 +778,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame>& frame,
   if (image.empty() || image.cols != width || image.rows != height) {
     image.create(height, width, image_format_[stream_index]);
   }
-  image.data = (uchar*)video_frame->data();
+  if (hw_decode) {
+    memcpy(image.data, rgb_buffer_, video_frame->width() * video_frame->height() * 3);
+  } else {
+    memcpy(image.data, video_frame->data(), video_frame->dataSize());
+  }
   if (stream_index == DEPTH) {
     auto depth_scale = video_frame->as<ob::DepthFrame>()->getValueScale();
     image = image * depth_scale;

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -16,6 +16,7 @@
 #include <sys/shm.h>
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
+#include <signal.h>
 
 namespace orbbec_camera {
 OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
@@ -77,6 +78,15 @@ void OBCameraNodeDriver::init() {
   device_count_update_thread_ = std::make_shared<std::thread>([this]() { deviceCountUpdate(); });
   sync_time_thread_ = std::make_shared<std::thread>([this]() { syncTime(); });
   reset_device_thread_ = std::make_shared<std::thread>([this]() { resetDevice(); });
+  signal(SIGINT, [](int) {
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("orbbec_camera_node_driver"), "SIGINT received");
+    exit(0);
+  });
+  signal(SIGTERM, [](int) {
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("orbbec_camera_node_driver"), "SIGTERM received");
+    exit(0);
+  });
+
 }
 
 void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) {

orbbec_camera/src/rk_mpp_decoder.cpp

@@ -0,0 +1,206 @@
+#include "orbbec_camera/rk_mpp_decoder.h"
+#include <rclcpp/rclcpp.hpp>
+#include <glog/logging.h>
+
+namespace orbbec_camera {
+
+RKMjpegDecoder::RKMjpegDecoder(int width, int height) : MjpegDecoder(width, height) {
+  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);
+    throw std::runtime_error("mpp_create failed");
+  }
+  MpiCmd mpi_cmd = MPP_CMD_BASE;
+  MppParam mpp_param = nullptr;
+
+  mpi_cmd = MPP_DEC_SET_PARSER_SPLIT_MODE;
+  mpp_param = &need_split_;
+  ret = mpp_api_->control(mpp_ctx_, mpi_cmd, mpp_param);
+  if (ret != MPP_OK) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
+                        "mpp_api_->control failed, ret = " << ret);
+    throw std::runtime_error("mpp_api_->control failed");
+  }
+  ret = mpp_init(mpp_ctx_, MPP_CTX_DEC, MPP_VIDEO_CodingMJPEG);
+  if (ret != MPP_OK) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"), "mpp_init failed, ret = " << ret);
+    throw std::runtime_error("mpp_init failed");
+  }
+  MppFrameFormat fmt = MPP_FMT_YUV420SP_VU;
+  mpp_param = &fmt;
+  ret = mpp_api_->control(mpp_ctx_, MPP_DEC_SET_OUTPUT_FORMAT, mpp_param);
+  if (ret != MPP_OK) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
+                        "mpp_api_->control failed, ret = " << ret);
+    throw std::runtime_error("mpp_api_->control failed");
+  }
+  ret = mpp_frame_init(&mpp_frame_);
+  if (ret != MPP_OK) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
+                        "mpp_frame_init failed, ret = " << ret);
+    throw std::runtime_error("mpp_frame_init failed");
+  }
+  ret = mpp_buffer_group_get_internal(&mpp_frame_group_, MPP_BUFFER_TYPE_ION);
+  if (ret != MPP_OK) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
+                        "mpp_buffer_group_get_internal failed, ret = " << ret);
+    throw std::runtime_error("mpp_buffer_group_get_internal failed");
+  }
+  ret = mpp_buffer_group_get_internal(&mpp_packet_group_, MPP_BUFFER_TYPE_ION);
+  if (ret != MPP_OK) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
+                        "mpp_buffer_group_get_internal failed, ret = " << ret);
+    throw std::runtime_error("mpp_buffer_group_get_internal failed");
+  }
+  RK_U32 hor_stride = MPP_ALIGN(width_, 16);
+  RK_U32 ver_stride = MPP_ALIGN(height_, 16);
+  ret = mpp_buffer_get(mpp_frame_group_, &mpp_frame_buffer_, hor_stride * ver_stride * 4);
+  if (ret != MPP_OK) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
+                        "mpp_buffer_get failed, ret = " << ret);
+    throw std::runtime_error("mpp_buffer_get failed");
+  }
+  mpp_frame_set_buffer(mpp_frame_, mpp_frame_buffer_);
+  ret = mpp_buffer_get(mpp_packet_group_, &mpp_packet_buffer_, width_ * height_ * 3);
+  if (ret != MPP_OK) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
+                        "mpp_buffer_get failed, ret = " << ret);
+    throw std::runtime_error("mpp_buffer_get failed");
+  }
+  mpp_packet_init_with_buffer(&mpp_packet_, mpp_packet_buffer_);
+  data_buffer_ = (uint8_t *)mpp_buffer_get_ptr(mpp_packet_buffer_);
+}
+
+RKMjpegDecoder::~RKMjpegDecoder() {
+  if (mpp_frame_buffer_) {
+    mpp_buffer_put(mpp_frame_buffer_);
+    mpp_frame_buffer_ = nullptr;
+  }
+  if (mpp_packet_buffer_) {
+    mpp_buffer_put(mpp_packet_buffer_);
+    mpp_packet_buffer_ = nullptr;
+  }
+  if (mpp_frame_group_) {
+    mpp_buffer_group_put(mpp_frame_group_);
+    mpp_frame_group_ = nullptr;
+  }
+  if (mpp_packet_group_) {
+    mpp_buffer_group_put(mpp_packet_group_);
+    mpp_packet_group_ = nullptr;
+  }
+  if (mpp_frame_) {
+    mpp_frame_deinit(&mpp_frame_);
+    mpp_frame_ = nullptr;
+  }
+  if (mpp_packet_) {
+    mpp_packet_deinit(&mpp_packet_);
+    mpp_packet_ = nullptr;
+  }
+  if (mpp_ctx_) {
+    mpp_destroy(mpp_ctx_);
+    mpp_ctx_ = nullptr;
+  }
+}
+
+bool RKMjpegDecoder::mppFrame2RGB(const MppFrame frame, uint8_t *data) {
+  rga_info_t src_info;
+  rga_info_t dst_info;
+  // NOTE: memset to zero is MUST
+  memset(&src_info, 0, sizeof(rga_info_t));
+  memset(&dst_info, 0, sizeof(rga_info_t));
+  int width = mpp_frame_get_width(frame);
+  int height = mpp_frame_get_height(frame);
+  MppBuffer buffer = mpp_frame_get_buffer(frame);
+  CHECK_EQ(width, width_);
+  CHECK_EQ(height, height_);
+  CHECK_NOTNULL(data);
+  CHECK_EQ(width, width_);
+  CHECK_EQ(height, height_);
+  memset(data, 0, width * height * 3);
+  auto buffer_ptr = mpp_buffer_get_ptr(buffer);
+  src_info.fd = -1;
+  src_info.mmuFlag = 1;
+  src_info.virAddr = buffer_ptr;
+  src_info.format = RK_FORMAT_YCbCr_420_SP;
+  dst_info.fd = -1;
+  dst_info.mmuFlag = 1;
+  dst_info.virAddr = data;
+  dst_info.format = RK_FORMAT_RGB_888;
+  rga_set_rect(&src_info.rect, 0, 0, width, height, width, height, RK_FORMAT_YCbCr_420_SP);
+  rga_set_rect(&dst_info.rect, 0, 0, width, height, width, height, RK_FORMAT_RGB_888);
+  int ret = c_RkRgaBlit(&src_info, &dst_info, nullptr);
+  if (ret) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
+                        "c_RkRgaBlit error " << ret << " errno " << strerror(errno));
+    return false;
+  }
+  return true;
+}
+
+bool RKMjpegDecoder::decode(const std::shared_ptr<ob::ColorFrame> &frame, uint8_t *dest) {
+  MPP_RET ret = MPP_OK;
+  memset(data_buffer_, 0, width_ * height_ * 3);
+  memcpy(data_buffer_, frame->data(), frame->dataSize());
+  mpp_packet_set_pos(mpp_packet_, data_buffer_);
+  mpp_packet_set_length(mpp_packet_, frame->dataSize());
+  mpp_packet_set_eos(mpp_packet_);
+  CHECK_NOTNULL(mpp_ctx_);
+  ret = mpp_api_->poll(mpp_ctx_, MPP_PORT_INPUT, MPP_POLL_BLOCK);
+  if (ret != MPP_OK) {
+    RCLCPP_ERROR(rclcpp::get_logger("rk_mpp_decoder"), "mpp poll failed %d", ret);
+    return false;
+  }
+  ret = mpp_api_->dequeue(mpp_ctx_, MPP_PORT_INPUT, &mpp_task_);
+  if (ret != MPP_OK) {
+    RCLCPP_ERROR(rclcpp::get_logger("rk_mpp_decoder"), "mpp dequeue failed %d", ret);
+    return false;
+  }
+  mpp_task_meta_set_packet(mpp_task_, KEY_INPUT_PACKET, mpp_packet_);
+  mpp_task_meta_set_frame(mpp_task_, KEY_OUTPUT_FRAME, mpp_frame_);
+  ret = mpp_api_->enqueue(mpp_ctx_, MPP_PORT_INPUT, mpp_task_);
+  if (ret != MPP_OK) {
+    RCLCPP_ERROR(rclcpp::get_logger("rk_mpp_decoder"), "mpp enqueue failed %d", ret);
+    return false;
+  }
+  ret = mpp_api_->poll(mpp_ctx_, MPP_PORT_OUTPUT, MPP_POLL_BLOCK);
+  if (ret != MPP_OK) {
+    RCLCPP_ERROR(rclcpp::get_logger("rk_mpp_decoder"), "mpp poll failed %d", ret);
+    return false;
+  }
+  ret = mpp_api_->dequeue(mpp_ctx_, MPP_PORT_OUTPUT, &mpp_task_);
+  if (ret != MPP_OK) {
+    RCLCPP_ERROR(rclcpp::get_logger("rk_mpp_decoder"), "mpp dequeue failed %d", ret);
+    return false;
+  }
+  if (mpp_task_) {
+    MppFrame output_frame = nullptr;
+    mpp_task_meta_get_frame(mpp_task_, KEY_OUTPUT_FRAME, &output_frame);
+    if (mpp_frame_) {
+      int width = mpp_frame_get_width(mpp_frame_);
+      int height = mpp_frame_get_height(mpp_frame_);
+      if (width != width_ || height != height_) {
+        RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"),
+                            "mpp frame size error " << width << " " << height);
+        return false;
+      }
+      if (!mppFrame2RGB(mpp_frame_, rgb_buffer_)) {
+        RCLCPP_ERROR_STREAM(rclcpp::get_logger("rk_mpp_decoder"), "mpp frame to rgb error");
+        return false;
+      }
+      if (mpp_frame_get_eos(output_frame)) {
+        RCLCPP_INFO_STREAM(rclcpp::get_logger("rk_mpp_decoder"), "mpp frame get eos");
+      }
+    }
+    ret = mpp_api_->enqueue(mpp_ctx_, MPP_PORT_OUTPUT, mpp_task_);
+    if (ret != MPP_OK) {
+      RCLCPP_ERROR(rclcpp::get_logger("rk_mpp_decoder"), "mpp enqueue failed %d", ret);
+      return false;
+    }
+    CHECK_NOTNULL(dest);
+    memcpy(dest, rgb_buffer_, width_ * height_ * 3);
+    return true;
+  }
+  return false;
+}
+
+}  // namespace orbbec_camera