add mpp hardware decode mjpeg

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) {
-    if (!setupFormatConvertType(frame->format())) {
-      RCLCPP_ERROR(logger_, "Unsupported color format: %d", frame->format());
-      return;
+  bool hw_decode = false;
+  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)
+      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